00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
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
00054
00055 if (!nh_private_.getParam ("use_magnetic_field_msg", use_magnetic_field_msg_))
00056 {
00057 if (use_mag_)
00058 {
00059 ROS_WARN("Deprecation Warning: The parameter use_magnetic_field_msg was not set, default is 'false'.");
00060 ROS_WARN("Starting with ROS Jade, use_magnetic_field_msg will default to 'true'!");
00061 }
00062 use_magnetic_field_msg_ = false;
00063 }
00064
00065 std::string world_frame;
00066
00067 if (!nh_private_.getParam ("world_frame", world_frame)) {
00068 world_frame = "nwu";
00069 ROS_WARN("Deprecation Warning: The parameter world_frame was not set, default is 'nwu'.");
00070 ROS_WARN("Starting with ROS Lunar, world_frame will default to 'enu'!");
00071 }
00072
00073 if (world_frame == "ned") {
00074 world_frame_ = WorldFrame::NED;
00075 } else if (world_frame == "nwu"){
00076 world_frame_ = WorldFrame::NWU;
00077 } else if (world_frame == "enu"){
00078 world_frame_ = WorldFrame::ENU;
00079 } else {
00080 ROS_ERROR("The parameter world_frame was set to invalid value '%s'.", world_frame.c_str());
00081 ROS_ERROR("Valid values are 'enu', 'ned' and 'nwu'. Setting to 'enu'.");
00082 world_frame_ = WorldFrame::ENU;
00083 }
00084 filter_.setWorldFrame(world_frame_);
00085
00086
00087 if (constant_dt_ < 0.0)
00088 {
00089 ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
00090 constant_dt_ = 0.0;
00091 }
00092
00093
00094
00095 if (constant_dt_ == 0.0)
00096 ROS_INFO("Using dt computed from message headers");
00097 else
00098 ROS_INFO("Using constant dt of %f sec", constant_dt_);
00099
00100
00101 config_server_.reset(new FilterConfigServer(nh_private_));
00102 FilterConfigServer::CallbackType f = boost::bind(&ImuFilterRos::reconfigCallback, this, _1, _2);
00103 config_server_->setCallback(f);
00104
00105
00106 imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
00107 ros::names::resolve("imu") + "/data", 5);
00108
00109 if (publish_debug_topics_)
00110 {
00111 rpy_filtered_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
00112 ros::names::resolve("imu") + "/rpy/filtered", 5);
00113
00114 rpy_raw_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
00115 ros::names::resolve("imu") + "/rpy/raw", 5);
00116 }
00117
00118
00119
00120 int queue_size = 5;
00121
00122 imu_subscriber_.reset(new ImuSubscriber(
00123 nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
00124
00125 if (use_mag_)
00126 {
00127 if (use_magnetic_field_msg_)
00128 {
00129 mag_subscriber_.reset(new MagSubscriber(
00130 nh_, ros::names::resolve("imu") + "/mag", queue_size));
00131 }
00132 else
00133 {
00134 mag_subscriber_.reset(new MagSubscriber(
00135 nh_, ros::names::resolve("imu") + "/magnetic_field", queue_size));
00136
00137
00138
00139 mag_republisher_ = nh_.advertise<MagMsg>(
00140 ros::names::resolve("imu") + "/magnetic_field", 5);
00141 vector_mag_subscriber_.reset(new MagVectorSubscriber(
00142 nh_, ros::names::resolve("imu") + "/mag", queue_size));
00143 vector_mag_subscriber_->registerCallback(&ImuFilterRos::imuMagVectorCallback, this);
00144 }
00145
00146 sync_.reset(new Synchronizer(
00147 SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
00148 sync_->registerCallback(boost::bind(&ImuFilterRos::imuMagCallback, this, _1, _2));
00149 }
00150 else
00151 {
00152 imu_subscriber_->registerCallback(&ImuFilterRos::imuCallback, this);
00153 }
00154
00155 check_topics_timer_ = nh_.createTimer(ros::Duration(10.0), &ImuFilterRos::checkTopicsTimerCallback, this);
00156 }
00157
00158 ImuFilterRos::~ImuFilterRos()
00159 {
00160 ROS_INFO ("Destroying ImuFilter");
00161
00162
00163 check_topics_timer_.stop();
00164 }
00165
00166 void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
00167 {
00168 boost::mutex::scoped_lock lock(mutex_);
00169
00170 const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
00171 const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
00172
00173 ros::Time time = imu_msg_raw->header.stamp;
00174 imu_frame_ = imu_msg_raw->header.frame_id;
00175
00176 if (!initialized_)
00177 {
00178 check_topics_timer_.stop();
00179 ROS_INFO("First IMU message received.");
00180 }
00181
00182 if (!initialized_ || stateless_)
00183 {
00184 geometry_msgs::Quaternion init_q;
00185 StatelessOrientation::computeOrientation(world_frame_, lin_acc, init_q);
00186 filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
00187
00188
00189 last_time_ = time;
00190 initialized_ = true;
00191 }
00192
00193
00194 float dt;
00195 if (constant_dt_ > 0.0)
00196 dt = constant_dt_;
00197 else
00198 dt = (time - last_time_).toSec();
00199
00200 last_time_ = time;
00201
00202 if (!stateless_)
00203 filter_.madgwickAHRSupdateIMU(
00204 ang_vel.x, ang_vel.y, ang_vel.z,
00205 lin_acc.x, lin_acc.y, lin_acc.z,
00206 dt);
00207
00208 publishFilteredMsg(imu_msg_raw);
00209 if (publish_tf_)
00210 publishTransform(imu_msg_raw);
00211 }
00212
00213 void ImuFilterRos::imuMagCallback(
00214 const ImuMsg::ConstPtr& imu_msg_raw,
00215 const MagMsg::ConstPtr& mag_msg)
00216 {
00217 boost::mutex::scoped_lock lock(mutex_);
00218
00219 const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
00220 const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
00221 const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;
00222
00223 ros::Time time = imu_msg_raw->header.stamp;
00224 imu_frame_ = imu_msg_raw->header.frame_id;
00225
00226
00227 geometry_msgs::Vector3 mag_compensated;
00228 mag_compensated.x = mag_fld.x - mag_bias_.x;
00229 mag_compensated.y = mag_fld.y - mag_bias_.y;
00230 mag_compensated.z = mag_fld.z - mag_bias_.z;
00231
00232 double roll = 0.0;
00233 double pitch = 0.0;
00234 double yaw = 0.0;
00235
00236 if (!initialized_)
00237 {
00238 check_topics_timer_.stop();
00239 ROS_INFO("First pair of IMU and magnetometer messages received.");
00240 }
00241
00242 if (!initialized_ || stateless_)
00243 {
00244
00245 if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
00246 {
00247 return;
00248 }
00249
00250 geometry_msgs::Quaternion init_q;
00251 StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, init_q);
00252 filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
00253
00254 last_time_ = time;
00255 initialized_ = true;
00256 }
00257
00258
00259 float dt;
00260 if (constant_dt_ > 0.0)
00261 dt = constant_dt_;
00262 else
00263 dt = (time - last_time_).toSec();
00264
00265 last_time_ = time;
00266
00267 if (!stateless_)
00268 filter_.madgwickAHRSupdate(
00269 ang_vel.x, ang_vel.y, ang_vel.z,
00270 lin_acc.x, lin_acc.y, lin_acc.z,
00271 mag_compensated.x, mag_compensated.y, mag_compensated.z,
00272 dt);
00273
00274 publishFilteredMsg(imu_msg_raw);
00275 if (publish_tf_)
00276 publishTransform(imu_msg_raw);
00277
00278 if(publish_debug_topics_)
00279 {
00280 geometry_msgs::Quaternion orientation;
00281 if (StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, orientation))
00282 {
00283 tf2::Matrix3x3(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)).getRPY(roll, pitch, yaw, 0);
00284 publishRawMsg(time, roll, pitch, yaw);
00285 }
00286 }
00287 }
00288
00289 void ImuFilterRos::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
00290 {
00291 double q0,q1,q2,q3;
00292 filter_.getOrientation(q0,q1,q2,q3);
00293 geometry_msgs::TransformStamped transform;
00294 transform.header.stamp = imu_msg_raw->header.stamp;
00295 if (reverse_tf_)
00296 {
00297 transform.header.frame_id = imu_frame_;
00298 transform.child_frame_id = fixed_frame_;
00299 transform.transform.rotation.w = q0;
00300 transform.transform.rotation.x = -q1;
00301 transform.transform.rotation.y = -q2;
00302 transform.transform.rotation.z = -q3;
00303 }
00304 else {
00305 transform.header.frame_id = fixed_frame_;
00306 transform.child_frame_id = imu_frame_;
00307 transform.transform.rotation.w = q0;
00308 transform.transform.rotation.x = q1;
00309 transform.transform.rotation.y = q2;
00310 transform.transform.rotation.z = q3;
00311 }
00312 tf_broadcaster_.sendTransform(transform);
00313
00314 }
00315
00316 void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
00317 {
00318 double q0,q1,q2,q3;
00319 filter_.getOrientation(q0,q1,q2,q3);
00320
00321
00322 boost::shared_ptr<ImuMsg> imu_msg =
00323 boost::make_shared<ImuMsg>(*imu_msg_raw);
00324
00325 imu_msg->orientation.w = q0;
00326 imu_msg->orientation.x = q1;
00327 imu_msg->orientation.y = q2;
00328 imu_msg->orientation.z = q3;
00329
00330 imu_msg->orientation_covariance[0] = orientation_variance_;
00331 imu_msg->orientation_covariance[1] = 0.0;
00332 imu_msg->orientation_covariance[2] = 0.0;
00333 imu_msg->orientation_covariance[3] = 0.0;
00334 imu_msg->orientation_covariance[4] = orientation_variance_;
00335 imu_msg->orientation_covariance[5] = 0.0;
00336 imu_msg->orientation_covariance[6] = 0.0;
00337 imu_msg->orientation_covariance[7] = 0.0;
00338 imu_msg->orientation_covariance[8] = orientation_variance_;
00339
00340 imu_publisher_.publish(imu_msg);
00341
00342 if(publish_debug_topics_)
00343 {
00344 geometry_msgs::Vector3Stamped rpy;
00345 tf2::Matrix3x3(tf2::Quaternion(q1,q2,q3,q0)).getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
00346
00347 rpy.header = imu_msg_raw->header;
00348 rpy_filtered_debug_publisher_.publish(rpy);
00349 }
00350 }
00351
00352 void ImuFilterRos::publishRawMsg(const ros::Time& t,
00353 float roll, float pitch, float yaw)
00354 {
00355 geometry_msgs::Vector3Stamped rpy;
00356 rpy.vector.x = roll;
00357 rpy.vector.y = pitch;
00358 rpy.vector.z = yaw ;
00359 rpy.header.stamp = t;
00360 rpy.header.frame_id = imu_frame_;
00361 rpy_raw_debug_publisher_.publish(rpy);
00362 }
00363
00364
00365 void ImuFilterRos::reconfigCallback(FilterConfig& config, uint32_t level)
00366 {
00367 double gain, zeta;
00368 boost::mutex::scoped_lock lock(mutex_);
00369 gain = config.gain;
00370 zeta = config.zeta;
00371 filter_.setAlgorithmGain(gain);
00372 filter_.setDriftBiasGain(zeta);
00373 ROS_INFO("Imu filter gain set to %f", gain);
00374 ROS_INFO("Gyro drift bias set to %f", zeta);
00375 mag_bias_.x = config.mag_bias_x;
00376 mag_bias_.y = config.mag_bias_y;
00377 mag_bias_.z = config.mag_bias_z;
00378 orientation_variance_ = config.orientation_stddev * config.orientation_stddev;
00379 ROS_INFO("Magnetometer bias values: %f %f %f", mag_bias_.x, mag_bias_.y, mag_bias_.z);
00380 }
00381
00382 void ImuFilterRos::imuMagVectorCallback(const MagVectorMsg::ConstPtr& mag_vector_msg)
00383 {
00384 MagMsg mag_msg;
00385 mag_msg.header = mag_vector_msg->header;
00386 mag_msg.magnetic_field = mag_vector_msg->vector;
00387
00388 mag_republisher_.publish(mag_msg);
00389 }
00390
00391 void ImuFilterRos::checkTopicsTimerCallback(const ros::TimerEvent&)
00392 {
00393 if (use_mag_)
00394 ROS_WARN_STREAM("Still waiting for data on topics " << ros::names::resolve("imu") << "/data_raw"
00395 << " and " << ros::names::resolve("imu") << "/mag" << "...");
00396 else
00397 ROS_WARN_STREAM("Still waiting for data on topic " << ros::names::resolve("imu") << "/data_raw" << "...");
00398 }