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 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
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
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
00085
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
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
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
00110
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
00129
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
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
00180 last_time_ = time;
00181 initialized_ = true;
00182 }
00183
00184
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
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
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
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
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
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 }