本文整理汇总了C++中Orientation::get_yaw方法的典型用法代码示例。如果您正苦于以下问题：C++ Orientation::get_yaw方法的具体用法？C++ Orientation::get_yaw怎么用？C++ Orientation::get_yaw使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Orientation的用法示例。
在下文中一共展示了Orientation::get_yaw方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: SpatialDataHandler
/**
* Receive new IMU data, calculate the new orientation and publish data on the topic
*/
int SpatialDataHandler(CPhidgetSpatialHandle spatial, void *userptr, CPhidgetSpatial_SpatialEventDataHandle *data, int count)
{
sensor_msgs::Imu imu;
sensor_msgs::MagneticField mag;
imu.header.frame_id = "base_link";
imu.header.stamp = ros::Time::now();
mag.header.frame_id = "base_link";
mag.header.stamp = ros::Time::now();
for(int i = 0; i< count; i++)
{
if (data[i]->angularRate[0] != 0 || data[i]->angularRate[1] != 0 || data[i]->angularRate[2] != 0)
orientation_calculation.updateAngles((float*)&(data[i]->angularRate[0]), (float*)&(data[i]->acceleration), data[i]->timestamp.seconds + ((float)data[i]->timestamp.microseconds)/1000000);
// Save info into the message and publish
imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(orientation_calculation.get_roll(),orientation_calculation.get_pitch(), orientation_calculation.get_yaw());
// Set up the angular velocity field. the data->angularRate is in deg/sec while we need the unit to be in rad/s
imu.angular_velocity.x = data[i]->angularRate[0] * (3.14 / 180);
imu.angular_velocity.y = data[i]->angularRate[1] * (3.14 / 180);
imu.angular_velocity.z = data[i]->angularRate[2] * (3.14 / 180);
// Set up the acceleration field. The data->acceleration is in g while we need the unit to be in m/s^2
imu.linear_acceleration.x = data[i]->acceleration[0] * 9.81;
imu.linear_acceleration.y = data[i]->acceleration[1] * 9.81;
imu.linear_acceleration.z = data[i]->acceleration[2] * 9.81;
// Set up the magnetic_field field. The data->magneticField is in Gauss while we need the unit to be in Tesla.
mag.magnetic_field.x = data[i]->magneticField[0] * 0.0001;
mag.magnetic_field.y = data[i]->magneticField[1] * 0.0001;
mag.magnetic_field.z = data[i]->magneticField[2] * 0.0001;
if(imu_pub)
imu_pub.publish(imu);
if(mag_pub)
mag_pub.publish(mag);
}
spatialError = 0;
return 0;
}