00001
00009
00010
00011
00012
00013
00014
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
00030
00031
00032
00033
00034
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
00071
00072
00073
00074 imu_nh.param<std::string>("frame_id", frame_id, "base_link");
00075 imu_nh.param("linear_acceleration_stdev", linear_stdev, 0.0003);
00076 imu_nh.param("angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0));
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
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
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
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
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
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
00174 imu_msg->header = header;
00175
00176 tf::vectorEigenToMsg(gyro, imu_msg->angular_velocity);
00177 tf::vectorEigenToMsg(accel, imu_msg->linear_acceleration);
00178
00179
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
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
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
00212
00213
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
00219
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
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
00235
00236
00237
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
00242
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
00260
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
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
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
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
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 };
00384
00385 PLUGINLIB_EXPORT_CLASS(mavplugin::IMUPubPlugin, mavplugin::MavRosPlugin)
00386