imu_filter_ros.cpp
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2010, CCNY Robotics Lab
00003  *  Ivan Dryanovski <ivan.dryanovski@gmail.com>
00004  *
00005  *  http://robotics.ccny.cuny.edu
00006  *
00007  *  Based on implementation of Madgwick's IMU and AHRS algorithms.
00008  *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
00009  *
00010  *
00011  *  This program is free software: you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation, either version 3 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU General Public License for more details.
00020  *
00021  *  You should have received a copy of the GNU General Public License
00022  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00023  */
00024 
00025 #include "imu_filter_madgwick/imu_filter_ros.h"
00026 #include "imu_filter_madgwick/stateless_orientation.h"
00027 #include "geometry_msgs/TransformStamped.h"
00028 #include <tf2/LinearMath/Quaternion.h>
00029 #include <tf2/LinearMath/Matrix3x3.h>
00030 
00031 ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private):
00032   nh_(nh),
00033   nh_private_(nh_private),
00034   initialized_(false)
00035 {
00036   ROS_INFO ("Starting ImuFilter");
00037 
00038   // **** get paramters
00039   if (!nh_private_.getParam ("stateless", stateless_))
00040     stateless_ = false;
00041   if (!nh_private_.getParam ("use_mag", use_mag_))
00042    use_mag_ = true;
00043   if (!nh_private_.getParam ("publish_tf", publish_tf_))
00044    publish_tf_ = true;
00045   if (!nh_private_.getParam ("reverse_tf", reverse_tf_))
00046    reverse_tf_ = false;
00047   if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
00048    fixed_frame_ = "odom";
00049   if (!nh_private_.getParam ("constant_dt", constant_dt_))
00050     constant_dt_ = 0.0;
00051   if (!nh_private_.getParam ("publish_debug_topics", publish_debug_topics_))
00052     publish_debug_topics_= false;
00053   if (!nh_private_.getParam ("use_magnetic_field_msg", use_magnetic_field_msg_))
00054     use_magnetic_field_msg_ = true;
00055 
00056   std::string world_frame;
00057   // Default should become false for next release
00058   if (!nh_private_.getParam ("world_frame", world_frame)) {
00059     world_frame = "nwu";
00060     ROS_WARN("Deprecation Warning: The parameter world_frame was not set, default is 'nwu'.");
00061     ROS_WARN("Starting with ROS Lunar, world_frame will default to 'enu'!");
00062   }
00063 
00064   if (world_frame == "ned") {
00065     world_frame_ = WorldFrame::NED;
00066   } else if (world_frame == "nwu"){
00067     world_frame_ = WorldFrame::NWU;
00068   } else if (world_frame == "enu"){
00069     world_frame_ = WorldFrame::ENU;
00070   } else {
00071     ROS_ERROR("The parameter world_frame was set to invalid value '%s'.", world_frame.c_str());
00072     ROS_ERROR("Valid values are 'enu', 'ned' and 'nwu'. Setting to 'enu'.");
00073     world_frame_ = WorldFrame::ENU;
00074   }
00075   filter_.setWorldFrame(world_frame_);
00076 
00077   // check for illegal constant_dt values
00078   if (constant_dt_ < 0.0)
00079   {
00080     ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
00081     constant_dt_ = 0.0;
00082   }
00083 
00084   // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt
00085   // otherwise, it will be constant
00086   if (constant_dt_ == 0.0)
00087     ROS_INFO("Using dt computed from message headers");
00088   else
00089     ROS_INFO("Using constant dt of %f sec", constant_dt_);
00090 
00091   // **** register dynamic reconfigure
00092   config_server_.reset(new FilterConfigServer(nh_private_));
00093   FilterConfigServer::CallbackType f = boost::bind(&ImuFilterRos::reconfigCallback, this, _1, _2);
00094   config_server_->setCallback(f);
00095 
00096   // **** register publishers
00097   imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
00098     ros::names::resolve("imu") + "/data", 5);
00099 
00100   if (publish_debug_topics_)
00101   {
00102     rpy_filtered_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
00103       ros::names::resolve("imu") + "/rpy/filtered", 5);
00104 
00105     rpy_raw_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
00106       ros::names::resolve("imu") + "/rpy/raw", 5);
00107   }
00108 
00109   // **** register subscribers
00110   // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
00111   int queue_size = 5;
00112 
00113   imu_subscriber_.reset(new ImuSubscriber(
00114     nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
00115 
00116   if (use_mag_)
00117   {
00118     if (use_magnetic_field_msg_)
00119     {
00120       mag_subscriber_.reset(new MagSubscriber(
00121         nh_, ros::names::resolve("imu") + "/mag", queue_size));
00122     }
00123     else
00124     {
00125       mag_subscriber_.reset(new MagSubscriber(
00126         nh_, ros::names::resolve("imu") + "/magnetic_field", queue_size));
00127 
00128       // Initialize the shim to support republishing Vector3Stamped messages from /mag as MagneticField
00129       // messages on the /magnetic_field topic.
00130       mag_republisher_ = nh_.advertise<MagMsg>(
00131         ros::names::resolve("imu") + "/magnetic_field", 5);
00132       vector_mag_subscriber_.reset(new MagVectorSubscriber(
00133         nh_, ros::names::resolve("imu") + "/mag", queue_size));
00134       vector_mag_subscriber_->registerCallback(&ImuFilterRos::imuMagVectorCallback, this);
00135     }
00136 
00137     sync_.reset(new Synchronizer(
00138       SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
00139     sync_->registerCallback(boost::bind(&ImuFilterRos::imuMagCallback, this, _1, _2));
00140   }
00141   else
00142   {
00143     imu_subscriber_->registerCallback(&ImuFilterRos::imuCallback, this);
00144   }
00145 
00146   check_topics_timer_ = nh_.createTimer(ros::Duration(10.0), &ImuFilterRos::checkTopicsTimerCallback, this);
00147 }
00148 
00149 ImuFilterRos::~ImuFilterRos()
00150 {
00151   ROS_INFO ("Destroying ImuFilter");
00152 
00153   // Explicitly stop callbacks; they could execute after we're destroyed
00154   check_topics_timer_.stop();
00155 }
00156 
00157 void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
00158 {
00159   boost::mutex::scoped_lock lock(mutex_);
00160 
00161   const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
00162   const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
00163 
00164   ros::Time time = imu_msg_raw->header.stamp;
00165   imu_frame_ = imu_msg_raw->header.frame_id;
00166 
00167   if (!initialized_)
00168   {
00169     check_topics_timer_.stop();
00170     ROS_INFO("First IMU message received.");
00171   }
00172 
00173   if (!initialized_ || stateless_)
00174   {
00175     geometry_msgs::Quaternion init_q;
00176     StatelessOrientation::computeOrientation(world_frame_, lin_acc, init_q);
00177     filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
00178 
00179     // initialize time
00180     last_time_ = time;
00181     initialized_ = true;
00182   }
00183 
00184   // determine dt: either constant, or from IMU timestamp
00185   float dt;
00186   if (constant_dt_ > 0.0)
00187     dt = constant_dt_;
00188   else
00189     dt = (time - last_time_).toSec();
00190 
00191   last_time_ = time;
00192 
00193   if (!stateless_)
00194     filter_.madgwickAHRSupdateIMU(
00195       ang_vel.x, ang_vel.y, ang_vel.z,
00196       lin_acc.x, lin_acc.y, lin_acc.z,
00197       dt);
00198 
00199   publishFilteredMsg(imu_msg_raw);
00200   if (publish_tf_)
00201     publishTransform(imu_msg_raw);
00202 }
00203 
00204 void ImuFilterRos::imuMagCallback(
00205   const ImuMsg::ConstPtr& imu_msg_raw,
00206   const MagMsg::ConstPtr& mag_msg)
00207 {
00208   boost::mutex::scoped_lock lock(mutex_);
00209 
00210   const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
00211   const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
00212   const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;
00213 
00214   ros::Time time = imu_msg_raw->header.stamp;
00215   imu_frame_ = imu_msg_raw->header.frame_id;
00216 
00217   /*** Compensate for hard iron ***/
00218   geometry_msgs::Vector3 mag_compensated;
00219   mag_compensated.x = mag_fld.x - mag_bias_.x;
00220   mag_compensated.y = mag_fld.y - mag_bias_.y;
00221   mag_compensated.z = mag_fld.z - mag_bias_.z;
00222 
00223   double roll = 0.0;
00224   double pitch = 0.0;
00225   double yaw = 0.0;
00226 
00227   if (!initialized_)
00228   {
00229     check_topics_timer_.stop();
00230     ROS_INFO("First pair of IMU and magnetometer messages received.");
00231   }
00232 
00233   if (!initialized_ || stateless_)
00234   {
00235     // wait for mag message without NaN / inf
00236     if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
00237     {
00238       return;
00239     }
00240 
00241     geometry_msgs::Quaternion init_q;
00242     StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, init_q);
00243     filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
00244 
00245     last_time_ = time;
00246     initialized_ = true;
00247   }
00248 
00249   // determine dt: either constant, or from IMU timestamp
00250   float dt;
00251   if (constant_dt_ > 0.0)
00252     dt = constant_dt_;
00253   else
00254     dt = (time - last_time_).toSec();
00255 
00256   last_time_ = time;
00257 
00258   if (!stateless_)
00259     filter_.madgwickAHRSupdate(
00260       ang_vel.x, ang_vel.y, ang_vel.z,
00261       lin_acc.x, lin_acc.y, lin_acc.z,
00262       mag_compensated.x, mag_compensated.y, mag_compensated.z,
00263       dt);
00264 
00265   publishFilteredMsg(imu_msg_raw);
00266   if (publish_tf_)
00267     publishTransform(imu_msg_raw);
00268 
00269   if(publish_debug_topics_)
00270   {
00271     geometry_msgs::Quaternion orientation;
00272     if (StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, orientation))
00273     {
00274       tf2::Matrix3x3(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)).getRPY(roll, pitch, yaw, 0);
00275       publishRawMsg(time, roll, pitch, yaw);
00276     }
00277   }
00278 }
00279 
00280 void ImuFilterRos::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
00281 {
00282   double q0,q1,q2,q3;
00283   filter_.getOrientation(q0,q1,q2,q3);
00284   geometry_msgs::TransformStamped transform;
00285   transform.header.stamp = imu_msg_raw->header.stamp;
00286   if (reverse_tf_)
00287   {
00288     transform.header.frame_id = imu_frame_;
00289     transform.child_frame_id = fixed_frame_;
00290     transform.transform.rotation.w = q0;
00291     transform.transform.rotation.x = -q1;
00292     transform.transform.rotation.y = -q2;
00293     transform.transform.rotation.z = -q3;
00294   }
00295   else {
00296     transform.header.frame_id = fixed_frame_;
00297     transform.child_frame_id = imu_frame_;
00298     transform.transform.rotation.w = q0;
00299     transform.transform.rotation.x = q1;
00300     transform.transform.rotation.y = q2;
00301     transform.transform.rotation.z = q3;
00302   }
00303   tf_broadcaster_.sendTransform(transform);
00304 
00305 }
00306 
00307 void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
00308 {
00309   double q0,q1,q2,q3;
00310   filter_.getOrientation(q0,q1,q2,q3);
00311 
00312   // create and publish filtered IMU message
00313   boost::shared_ptr<ImuMsg> imu_msg =
00314     boost::make_shared<ImuMsg>(*imu_msg_raw);
00315 
00316   imu_msg->orientation.w = q0;
00317   imu_msg->orientation.x = q1;
00318   imu_msg->orientation.y = q2;
00319   imu_msg->orientation.z = q3;
00320 
00321   imu_msg->orientation_covariance[0] = orientation_variance_;
00322   imu_msg->orientation_covariance[1] = 0.0;
00323   imu_msg->orientation_covariance[2] = 0.0;
00324   imu_msg->orientation_covariance[3] = 0.0;
00325   imu_msg->orientation_covariance[4] = orientation_variance_;
00326   imu_msg->orientation_covariance[5] = 0.0;
00327   imu_msg->orientation_covariance[6] = 0.0;
00328   imu_msg->orientation_covariance[7] = 0.0;
00329   imu_msg->orientation_covariance[8] = orientation_variance_;
00330 
00331   imu_publisher_.publish(imu_msg);
00332 
00333   if(publish_debug_topics_)
00334   {
00335     geometry_msgs::Vector3Stamped rpy;
00336     tf2::Matrix3x3(tf2::Quaternion(q1,q2,q3,q0)).getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
00337 
00338     rpy.header = imu_msg_raw->header;
00339     rpy_filtered_debug_publisher_.publish(rpy);
00340   }
00341 }
00342 
00343 void ImuFilterRos::publishRawMsg(const ros::Time& t,
00344   float roll, float pitch, float yaw)
00345 {
00346   geometry_msgs::Vector3Stamped rpy;
00347   rpy.vector.x = roll;
00348   rpy.vector.y = pitch;
00349   rpy.vector.z = yaw ;
00350   rpy.header.stamp = t;
00351   rpy.header.frame_id = imu_frame_;
00352   rpy_raw_debug_publisher_.publish(rpy);
00353 }
00354 
00355 
00356 void ImuFilterRos::reconfigCallback(FilterConfig& config, uint32_t level)
00357 {
00358   double gain, zeta;
00359   boost::mutex::scoped_lock lock(mutex_);
00360   gain = config.gain;
00361   zeta = config.zeta;
00362   filter_.setAlgorithmGain(gain);
00363   filter_.setDriftBiasGain(zeta);
00364   ROS_INFO("Imu filter gain set to %f", gain);
00365   ROS_INFO("Gyro drift bias set to %f", zeta);
00366   mag_bias_.x = config.mag_bias_x;
00367   mag_bias_.y = config.mag_bias_y;
00368   mag_bias_.z = config.mag_bias_z;
00369   orientation_variance_ = config.orientation_stddev * config.orientation_stddev;
00370   ROS_INFO("Magnetometer bias values: %f %f %f", mag_bias_.x, mag_bias_.y, mag_bias_.z);
00371 }
00372 
00373 void ImuFilterRos::imuMagVectorCallback(const MagVectorMsg::ConstPtr& mag_vector_msg)
00374 {
00375   MagMsg mag_msg;
00376   mag_msg.header = mag_vector_msg->header;
00377   mag_msg.magnetic_field = mag_vector_msg->vector;
00378   // leaving mag_msg.magnetic_field_covariance set to all zeros (= "covariance unknown")
00379   mag_republisher_.publish(mag_msg);
00380 }
00381 
00382 void ImuFilterRos::checkTopicsTimerCallback(const ros::TimerEvent&)
00383 {
00384   if (use_mag_)
00385     ROS_WARN_STREAM("Still waiting for data on topics " << ros::names::resolve("imu") << "/data_raw"
00386                     << " and " << ros::names::resolve("imu") << "/mag" << "...");
00387   else
00388     ROS_WARN_STREAM("Still waiting for data on topic " << ros::names::resolve("imu") << "/data_raw" << "...");
00389 }


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Tue May 23 2017 02:23:02