bool spin()
{
ros::Rate loop_rate(rate_);
while (ros::ok())
{
if (burst_mode_)
{
if (imu.update_burst() == 0)
{
publish_imu_data();
}
else
{
ROS_ERROR("Cannot update burst");
}
}
else
{
if (imu.update() == 0)
{
publish_imu_data();
}
else
{
ROS_ERROR("Cannot update");
}
}
ros::spinOnce();
loop_rate.sleep();
}
return true;
}
/**
* @brief Handle ATTITUDE_QUATERNION MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html/#ATTITUDE_QUATERNION
* @param msg Received Mavlink msg
* @param att_q ATTITUDE_QUATERNION msg
*/
void handle_attitude_quaternion(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE_QUATERNION &att_q)
{
ROS_INFO_COND_NAMED(!has_att_quat, "imu", "IMU: Attitude quaternion IMU detected!");
has_att_quat = true;
/** Orientation on the NED-aicraft frame:
* @snippet src/plugins/imu.cpp ned_aircraft_orient2
*/
// [ned_aircraft_orient2]
auto ned_aircraft_orientation = Eigen::Quaterniond(att_q.q1, att_q.q2, att_q.q3, att_q.q4);
// [ned_aircraft_orient2]
/** Angular velocity on the NED-aicraft frame:
* @snippet src/plugins/imu.cpp ned_ang_vel2
*/
// [frd_ang_vel2]
auto gyro_frd = Eigen::Vector3d(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed);
// [frd_ang_vel2]
/** MAVLink quaternion exactly matches Eigen convention.
* The RPY describes the rotation: aircraft->NED.
* It is required to change this to aircraft->base_link:
* @snippet src/plugins/imu.cpp ned->baselink->enu
*/
auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
ftf::transform_orientation_ned_enu(ned_aircraft_orientation));
/** The angular velocity expressed in the aircraft frame.
* It is required to apply the static rotation to get it into the base_link frame:
* @snippet src/plugins/imu.cpp rotate_gyro
*/
auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd);
publish_imu_data(att_q.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu, gyro_frd);
}
/**
* @brief Handle ATTITUDE MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html#ATTITUDE
* @param msg Received Mavlink msg
* @param att ATTITUDE msg
*/
void handle_attitude(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE &att)
{
if (has_att_quat)
return;
/** Orientation on the NED-aicraft frame:
* @snippet src/plugins/imu.cpp ned_aircraft_orient1
*/
// [ned_aircraft_orient1]
auto ned_aircraft_orientation = ftf::quaternion_from_rpy(att.roll, att.pitch, att.yaw);
// [ned_aircraft_orient1]
/** Angular velocity on the NED-aicraft frame:
* @snippet src/plugins/imu.cpp ned_ang_vel1
*/
// [frd_ang_vel1]
auto gyro_frd = Eigen::Vector3d(att.rollspeed, att.pitchspeed, att.yawspeed);
// [frd_ang_vel1]
/** The RPY describes the rotation: aircraft->NED.
* It is required to change this to aircraft->base_link:
* @snippet src/plugins/imu.cpp ned->baselink->enu
*/
// [ned->baselink->enu]
auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
ftf::transform_orientation_ned_enu(ned_aircraft_orientation));
// [ned->baselink->enu]
/** The angular velocity expressed in the aircraft frame.
* It is required to apply the static rotation to get it into the base_link frame:
* @snippet src/plugins/imu.cpp rotate_gyro
*/
// [rotate_gyro]
auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd);
// [rotate_gyro]
publish_imu_data(att.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu, gyro_frd);
}
