本文整理汇总了C++中Filter::getValue方法的典型用法代码示例。如果您正苦于以下问题：C++ Filter::getValue方法的具体用法？C++ Filter::getValue怎么用？C++ Filter::getValue使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Filter的用法示例。
在下文中一共展示了Filter::getValue方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: publishState
void publishState(void)
{
uint8_t buf[10];
const int ret = libusb_control_transfer(dev, 0xC0, 0x32, 0x0, 0x0, buf, 10, 0);
if (ret != 10)
{
ROS_ERROR_STREAM("Error in accelerometer reading, libusb_control_transfer returned " << ret);
ros::shutdown();
}
const uint16_t ux = ((uint16_t)buf[2] << 8) | buf[3];
const uint16_t uy = ((uint16_t)buf[4] << 8) | buf[5];
const uint16_t uz = ((uint16_t)buf[6] << 8) | buf[7];
const double accelerometer_x = (int16_t)ux;
const double accelerometer_y = (int16_t)uy;
const double accelerometer_z = ((int16_t)uz);
const int8_t tilt_angle = (int8_t)buf[8];
const uint8_t tilt_status = buf[9];
// publish IMU
sensor_msgs::Imu imu_msg;
if (pub_imu.getNumSubscribers() > 0)
{
imu_msg.header.stamp = ros::Time::now();
imu_msg.linear_acceleration.x = filters[0].getValue(interpolators[0].getValue(accelerometer_x) * GRAVITY);
imu_msg.linear_acceleration.y = filters[1].getValue(interpolators[1].getValue(accelerometer_y) * GRAVITY);
imu_msg.linear_acceleration.z = filters[2].getValue(interpolators[2].getValue(accelerometer_z) * GRAVITY);
imu_msg.linear_acceleration_covariance[0] = imu_msg.linear_acceleration_covariance[4]
= imu_msg.linear_acceleration_covariance[8] = 0.01; // @todo - what should these be?
imu_msg.angular_velocity_covariance[0] = -1; // indicates angular velocity not provided
imu_msg.orientation_covariance[0] = -1; // indicates orientation not provided
double magnitude = sqrt((imu_msg.linear_acceleration.x * imu_msg.linear_acceleration.x) + (imu_msg.linear_acceleration.y * imu_msg.linear_acceleration.y) + (imu_msg.linear_acceleration.z + imu_msg.linear_acceleration.z));
//Only publish when the summed magnitued is within 15% of the expected force due to gravity
double error = magnitude - GRAVITY;
double delta = magnitude - lastForce;
double deltaExpected = deltaFilter.getValue(delta);
if(abs(delta - deltaExpected) < (0.1 * GRAVITY) && abs(error) < (GRAVITY * ERROR_MARGIN)){
tf::Quaternion q;
double pitch = -atan2( imu_msg.linear_acceleration.z, sqrt((imu_msg.linear_acceleration.x * imu_msg.linear_acceleration.x) + (imu_msg.linear_acceleration.y * imu_msg.linear_acceleration.y)) );
double roll = atan2( imu_msg.linear_acceleration.x, sqrt((imu_msg.linear_acceleration.z * imu_msg.linear_acceleration.z) + (imu_msg.linear_acceleration.y * imu_msg.linear_acceleration.y)) );
q.setRPY( roll, pitch, 0.0 );
imu_msg.header.frame_id = "/camera_link";
imu_msg.orientation.x = q.x();
imu_msg.orientation.y = q.y();
imu_msg.orientation.z = q.z();
imu_msg.orientation.w = q.w();
pub_imu.publish(imu_msg);
}
}
// publish tilt angle and status
if (pub_tilt_angle.getNumSubscribers() > 0)
{
std_msgs::Float64 tilt_angle_msg;
tilt_angle_msg.data = double(tilt_angle) / 2.;
pub_tilt_angle.publish(tilt_angle_msg);
}
if (pub_tilt_status.getNumSubscribers() > 0)
{
std_msgs::UInt8 tilt_status_msg;
tilt_status_msg.data = tilt_status;
pub_tilt_status.publish(tilt_status_msg);
}
}