imu_pub.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2013,2015 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 
00017 #include <cmath>
00018 #include <mavros/mavros_plugin.h>
00019 #include <pluginlib/class_list_macros.h>
00020 #include <eigen_conversions/eigen_msg.h>
00021 
00022 #include <sensor_msgs/Imu.h>
00023 #include <sensor_msgs/MagneticField.h>
00024 #include <sensor_msgs/Temperature.h>
00025 #include <sensor_msgs/FluidPressure.h>
00026 #include <geometry_msgs/Vector3.h>
00027 
00028 namespace mavplugin {
00029 /* Note: this coefficents before been inside plugin class,
00030  * but after #320 something broken and in resulting plugins.so
00031  * there no symbols for that constants.
00032  * That cause plugin loader failure.
00033  *
00034  * objdump -x plugins.so | grep MILLI
00035  */
00036 
00038 static constexpr double GAUSS_TO_TESLA = 1.0e-4;
00040 static constexpr double MILLIT_TO_TESLA = 1000.0;
00042 static constexpr double MILLIRS_TO_RADSEC = 1.0e-3;
00044 static constexpr double MILLIG_TO_MS2 = 9.80665 / 1000.0;
00046 static constexpr double MILLIBAR_TO_PASCAL = 1.0e2;
00047 
00048 static constexpr double RAD_TO_DEG = 180.0 / M_PI;
00049 
00050 
00054 class IMUPubPlugin : public MavRosPlugin {
00055 public:
00056         IMUPubPlugin() :
00057                 imu_nh("~imu"),
00058                 uas(nullptr),
00059                 has_hr_imu(false),
00060                 has_scaled_imu(false),
00061                 has_att_quat(false)
00062         { };
00063 
00064         void initialize(UAS &uas_)
00065         {
00066                 double linear_stdev, angular_stdev, orientation_stdev, mag_stdev;
00067 
00068                 uas = &uas_;
00069 
00070                 // we rotate the data from the aircraft-frame to the base_link frame.
00071                 // Additionally we report the orientation of the vehicle to describe the
00072                 // transformation from the ENU frame to the base_link frame (ENU <-> base_link).
00073                 // THIS ORIENTATION IS NOT THE SAME AS THAT REPORTED BY THE FCU (NED <-> aircraft)
00074                 imu_nh.param<std::string>("frame_id", frame_id, "base_link");
00075                 imu_nh.param("linear_acceleration_stdev", linear_stdev, 0.0003);                // check default by MPU6000 spec
00076                 imu_nh.param("angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0));   // check default by MPU6000 spec
00077                 imu_nh.param("orientation_stdev", orientation_stdev, 1.0);
00078                 imu_nh.param("magnetic_stdev", mag_stdev, 0.0);
00079 
00080                 setup_covariance(linear_acceleration_cov, linear_stdev);
00081                 setup_covariance(angular_velocity_cov, angular_stdev);
00082                 setup_covariance(orientation_cov, orientation_stdev);
00083                 setup_covariance(magnetic_cov, mag_stdev);
00084                 setup_covariance(unk_orientation_cov, 0.0);
00085 
00086                 imu_pub = imu_nh.advertise<sensor_msgs::Imu>("data", 10);
00087                 magn_pub = imu_nh.advertise<sensor_msgs::MagneticField>("mag", 10);
00088                 temp_pub = imu_nh.advertise<sensor_msgs::Temperature>("temperature", 10);
00089                 press_pub = imu_nh.advertise<sensor_msgs::FluidPressure>("atm_pressure", 10);
00090                 imu_raw_pub = imu_nh.advertise<sensor_msgs::Imu>("data_raw", 10);
00091 
00092                 // reset has_* flags on connection change
00093                 uas->sig_connection_changed.connect(boost::bind(&IMUPubPlugin::connection_cb, this, _1));
00094         }
00095 
00096         const message_map get_rx_handlers() {
00097                 return {
00098                                MESSAGE_HANDLER(MAVLINK_MSG_ID_ATTITUDE, &IMUPubPlugin::handle_attitude),
00099                                MESSAGE_HANDLER(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &IMUPubPlugin::handle_attitude_quaternion),
00100                                MESSAGE_HANDLER(MAVLINK_MSG_ID_HIGHRES_IMU, &IMUPubPlugin::handle_highres_imu),
00101                                MESSAGE_HANDLER(MAVLINK_MSG_ID_RAW_IMU, &IMUPubPlugin::handle_raw_imu),
00102                                MESSAGE_HANDLER(MAVLINK_MSG_ID_SCALED_IMU, &IMUPubPlugin::handle_scaled_imu),
00103                                MESSAGE_HANDLER(MAVLINK_MSG_ID_SCALED_PRESSURE, &IMUPubPlugin::handle_scaled_pressure),
00104                 };
00105         }
00106 
00107 private:
00108         ros::NodeHandle imu_nh;
00109         UAS *uas;
00110         std::string frame_id;
00111 
00112         ros::Publisher imu_pub;
00113         ros::Publisher imu_raw_pub;
00114         ros::Publisher magn_pub;
00115         ros::Publisher temp_pub;
00116         ros::Publisher press_pub;
00117 
00118         bool has_hr_imu;
00119         bool has_scaled_imu;
00120         bool has_att_quat;
00121         Eigen::Vector3d linear_accel_vec;
00122         UAS::Covariance3d linear_acceleration_cov;
00123         UAS::Covariance3d angular_velocity_cov;
00124         UAS::Covariance3d orientation_cov;
00125         UAS::Covariance3d unk_orientation_cov;
00126         UAS::Covariance3d magnetic_cov;
00127 
00128         /* -*- helpers -*- */
00129 
00130         void setup_covariance(UAS::Covariance3d &cov, double stdev) {
00131                 std::fill(cov.begin(), cov.end(), 0.0);
00132                 if (stdev == 0.0)
00133                         cov[0] = -1.0;
00134                 else {
00135                         cov[0 + 0] = cov[3 + 1] = cov[6 + 2] = std::pow(stdev, 2);
00136                 }
00137         }
00138 
00140         void publish_imu_data(
00141                         uint32_t time_boot_ms,
00142                         Eigen::Quaterniond &orientation,
00143                         Eigen::Vector3d &gyro)
00144         {
00145                 auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
00146 
00147                 // fill
00148                 imu_msg->header = uas->synchronized_header(frame_id, time_boot_ms);
00149 
00150                 tf::quaternionEigenToMsg(orientation, imu_msg->orientation);
00151                 tf::vectorEigenToMsg(gyro, imu_msg->angular_velocity);
00152 
00153                 // vector from HIGHRES_IMU or RAW_IMU
00154                 tf::vectorEigenToMsg(linear_accel_vec, imu_msg->linear_acceleration);
00155 
00156                 imu_msg->orientation_covariance = orientation_cov;
00157                 imu_msg->angular_velocity_covariance = angular_velocity_cov;
00158                 imu_msg->linear_acceleration_covariance = linear_acceleration_cov;
00159 
00160                 // publish
00161                 uas->update_attitude_imu(imu_msg);
00162                 imu_pub.publish(imu_msg);
00163         }
00164 
00166         void publish_imu_data_raw(
00167                         std_msgs::Header &header,
00168                         Eigen::Vector3d &gyro,
00169                         Eigen::Vector3d &accel)
00170         {
00171                 auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
00172 
00173                 // fill
00174                 imu_msg->header = header;
00175 
00176                 tf::vectorEigenToMsg(gyro, imu_msg->angular_velocity);
00177                 tf::vectorEigenToMsg(accel, imu_msg->linear_acceleration);
00178 
00179                 // save readings
00180                 linear_accel_vec = accel;
00181 
00182                 imu_msg->orientation_covariance = unk_orientation_cov;
00183                 imu_msg->angular_velocity_covariance = angular_velocity_cov;
00184                 imu_msg->linear_acceleration_covariance = linear_acceleration_cov;
00185 
00186                 // publish
00187                 imu_raw_pub.publish(imu_msg);
00188         }
00189 
00190         void publish_mag(std_msgs::Header &header,
00191                         Eigen::Vector3d &mag_field)
00192         {
00193                 auto magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
00194 
00195                 magn_msg->header = header;
00196                 tf::vectorEigenToMsg(mag_field, magn_msg->magnetic_field);
00197                 magn_msg->magnetic_field_covariance = magnetic_cov;
00198 
00199                 magn_pub.publish(magn_msg);
00200         }
00201 
00202         /* -*- message handlers -*- */
00203 
00204         void handle_attitude(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00205                 if (has_att_quat)
00206                         return;
00207 
00208                 mavlink_attitude_t att;
00209                 mavlink_msg_attitude_decode(msg, &att);
00210 
00211                 //Here we have rpy describing the rotation: aircraft->NED
00212                 //We need to change this to aircraft->ENU
00213                 //And finally change it to baselink->ENU
00214                 auto enu_baselink_orientation = UAS::transform_orientation_aircraft_baselink(
00215                                 UAS::transform_orientation_ned_enu(
00216                                         UAS::quaternion_from_rpy(att.roll, att.pitch, att.yaw)));
00217 
00218                 //Here we have the angular velocity expressed in the aircraft frame
00219                 //We need to apply the static rotation to get it into the base_link frame
00220                 auto gyro = UAS::transform_frame_aircraft_baselink(
00221                                 Eigen::Vector3d(att.rollspeed, att.pitchspeed, att.yawspeed));
00222 
00223                 publish_imu_data(att.time_boot_ms, enu_baselink_orientation, gyro);
00224         }
00225 
00226         // almost the same as handle_attitude(), but for ATTITUDE_QUATERNION
00227         void handle_attitude_quaternion(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00228                 mavlink_attitude_quaternion_t att_q;
00229                 mavlink_msg_attitude_quaternion_decode(msg, &att_q);
00230 
00231                 ROS_INFO_COND_NAMED(!has_att_quat, "imu", "IMU: Attitude quaternion IMU detected!");
00232                 has_att_quat = true;
00233 
00234                 //MAVLink quaternion exactly matches Eigen convention
00235                 //Here we have rpy describing the rotation: aircraft->NED
00236                 //We need to change this to aircraft->ENU
00237                 //And finally change it to baselink->ENU
00238                 auto enu_baselink_orientation = UAS::transform_orientation_aircraft_baselink(
00239                                 UAS::transform_orientation_ned_enu(
00240                                         Eigen::Quaterniond(att_q.q1, att_q.q2, att_q.q3, att_q.q4)));
00241                 //Here we have the angular velocity expressed in the aircraft frame
00242                 //We need to apply the static rotation to get it into the base_link frame
00243                 auto gyro = UAS::transform_frame_aircraft_baselink(
00244                                 Eigen::Vector3d(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed));
00245 
00246                 publish_imu_data(att_q.time_boot_ms, enu_baselink_orientation, gyro);
00247         }
00248 
00249         void handle_highres_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00250                 mavlink_highres_imu_t imu_hr;
00251                 mavlink_msg_highres_imu_decode(msg, &imu_hr);
00252 
00253                 ROS_INFO_COND_NAMED(!has_hr_imu, "imu", "IMU: High resolution IMU detected!");
00254                 has_hr_imu = true;
00255 
00256                 auto header = uas->synchronized_header(frame_id, imu_hr.time_usec);
00258 
00259                 // accelerometer + gyroscope data available
00260                 // Data is expressed in aircraft frame we need to rotate to base_link frame
00261                 if (imu_hr.fields_updated & ((7 << 3) | (7 << 0))) {
00262                         auto gyro = UAS::transform_frame_aircraft_baselink(Eigen::Vector3d(imu_hr.xgyro, imu_hr.ygyro, imu_hr.zgyro));
00263                         auto accel = UAS::transform_frame_aircraft_baselink(Eigen::Vector3d(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc));
00264 
00265                         publish_imu_data_raw(header, gyro, accel);
00266                 }
00267 
00268                 // magnetometer data available
00269                 if (imu_hr.fields_updated & (7 << 6)) {
00270                         auto mag_field = UAS::transform_frame_aircraft_baselink<Eigen::Vector3d>(
00271                                         Eigen::Vector3d(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag) * GAUSS_TO_TESLA);
00272 
00273                         publish_mag(header, mag_field);
00274                 }
00275 
00276                 // pressure data available
00277                 if (imu_hr.fields_updated & (1 << 9)) {
00278                         auto atmp_msg = boost::make_shared<sensor_msgs::FluidPressure>();
00279 
00280                         atmp_msg->header = header;
00281                         atmp_msg->fluid_pressure = imu_hr.abs_pressure * MILLIBAR_TO_PASCAL;
00282 
00283                         press_pub.publish(atmp_msg);
00284                 }
00285 
00286                 if (imu_hr.fields_updated & (1 << 12)) {
00287                         auto temp_msg = boost::make_shared<sensor_msgs::Temperature>();
00288 
00289                         temp_msg->header = header;
00290                         temp_msg->temperature = imu_hr.temperature;
00291 
00292                         temp_pub.publish(temp_msg);
00293                 }
00294         }
00295 
00296         void handle_raw_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00297                 if (has_hr_imu || has_scaled_imu)
00298                         return;
00299 
00300                 auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
00301                 mavlink_raw_imu_t imu_raw;
00302                 mavlink_msg_raw_imu_decode(msg, &imu_raw);
00303 
00304                 auto header = uas->synchronized_header(frame_id, imu_raw.time_usec);
00305 
00307                 auto gyro = UAS::transform_frame_aircraft_baselink<Eigen::Vector3d>(
00308                                 Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
00309                 auto accel = UAS::transform_frame_aircraft_baselink<Eigen::Vector3d>(
00310                                 Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc));
00311 
00312                 if (uas->is_ardupilotmega())
00313                         accel *= MILLIG_TO_MS2;
00314 
00315                 publish_imu_data_raw(header, gyro, accel);
00316 
00317                 if (!uas->is_ardupilotmega()) {
00318                         ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: linear acceleration on RAW_IMU known on APM only.");
00319                         ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: ~imu/data_raw stores unscaled raw acceleration report.");
00320                         linear_accel_vec.setZero();
00321                 }
00322 
00323                 /* -*- magnetic vector -*- */
00324                 auto mag_field = UAS::transform_frame_aircraft_baselink<Eigen::Vector3d>(
00325                                 Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
00326 
00327                 publish_mag(header, mag_field);
00328         }
00329 
00330         void handle_scaled_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00331                 if (has_hr_imu)
00332                         return;
00333 
00334                 ROS_INFO_COND_NAMED(!has_scaled_imu, "imu", "IMU: Scaled IMU message used.");
00335                 has_scaled_imu = true;
00336 
00337                 auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
00338                 mavlink_scaled_imu_t imu_raw;
00339                 mavlink_msg_scaled_imu_decode(msg, &imu_raw);
00340 
00341                 auto header = uas->synchronized_header(frame_id, imu_raw.time_boot_ms);
00342 
00343                 auto gyro = UAS::transform_frame_aircraft_baselink<Eigen::Vector3d>(
00344                                 Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
00345                 auto accel = UAS::transform_frame_aircraft_baselink<Eigen::Vector3d>(
00346                                 Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc) * MILLIG_TO_MS2);
00347 
00348                 publish_imu_data_raw(header, gyro, accel);
00349 
00350                 /* -*- magnetic vector -*- */
00351                 auto mag_field = UAS::transform_frame_aircraft_baselink<Eigen::Vector3d>(
00352                                 Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
00353 
00354                 publish_mag(header, mag_field);
00355         }
00356 
00357         void handle_scaled_pressure(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00358                 if (has_hr_imu)
00359                         return;
00360 
00361                 mavlink_scaled_pressure_t press;
00362                 mavlink_msg_scaled_pressure_decode(msg, &press);
00363 
00364                 auto header = uas->synchronized_header(frame_id, press.time_boot_ms);
00365 
00366                 auto temp_msg = boost::make_shared<sensor_msgs::Temperature>();
00367                 temp_msg->header = header;
00368                 temp_msg->temperature = press.temperature / 100.0;
00369                 temp_pub.publish(temp_msg);
00370 
00371                 auto atmp_msg = boost::make_shared<sensor_msgs::FluidPressure>();
00372                 atmp_msg->header = header;
00373                 atmp_msg->fluid_pressure = press.press_abs * 100.0;
00374                 press_pub.publish(atmp_msg);
00375         }
00376 
00377         void connection_cb(bool connected) {
00378                 has_hr_imu = false;
00379                 has_scaled_imu = false;
00380                 has_att_quat = false;
00381         }
00382 };
00383 };      // namespace mavplugin
00384 
00385 PLUGINLIB_EXPORT_CLASS(mavplugin::IMUPubPlugin, mavplugin::MavRosPlugin)
00386 


mavros
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:17