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 "geometry_msgs/TransformStamped.h"
00027 #include <tf2/LinearMath/Quaternion.h>
00028 #include <tf2/LinearMath/Matrix3x3.h>
00029
00030 ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private):
00031 nh_(nh),
00032 nh_private_(nh_private),
00033 initialized_(false)
00034 {
00035 ROS_INFO ("Starting ImuFilter");
00036
00037
00038
00039 if (!nh_private_.getParam ("use_mag", use_mag_))
00040 use_mag_ = true;
00041 if (!nh_private_.getParam ("publish_tf", publish_tf_))
00042 publish_tf_ = true;
00043 if (!nh_private_.getParam ("reverse_tf", reverse_tf_))
00044 reverse_tf_ = false;
00045 if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
00046 fixed_frame_ = "odom";
00047 if (!nh_private_.getParam ("constant_dt", constant_dt_))
00048 constant_dt_ = 0.0;
00049 if (!nh_private_.getParam ("publish_debug_topics", publish_debug_topics_))
00050 publish_debug_topics_= false;
00051
00052
00053 if (!nh_private_.getParam ("use_magnetic_field_msg", use_magnetic_field_msg_))
00054 {
00055 if (use_mag_)
00056 {
00057 ROS_WARN("Deprecation Warning: The parameter use_magnetic_field_msg was not set, default is 'false'.");
00058 ROS_WARN("Starting with ROS Jade, use_magnetic_field_msg will default to 'true'!");
00059 }
00060 use_magnetic_field_msg_ = false;
00061 }
00062
00063
00064 if (constant_dt_ < 0.0)
00065 {
00066 ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
00067 constant_dt_ = 0.0;
00068 }
00069
00070
00071
00072 if (constant_dt_ == 0.0)
00073 ROS_INFO("Using dt computed from message headers");
00074 else
00075 ROS_INFO("Using constant dt of %f sec", constant_dt_);
00076
00077
00078 config_server_.reset(new FilterConfigServer(nh_private_));
00079 FilterConfigServer::CallbackType f = boost::bind(&ImuFilterRos::reconfigCallback, this, _1, _2);
00080 config_server_->setCallback(f);
00081
00082
00083 imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
00084 ros::names::resolve("imu") + "/data", 5);
00085
00086 if (publish_debug_topics_)
00087 {
00088 rpy_filtered_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
00089 ros::names::resolve("imu") + "/rpy/filtered", 5);
00090
00091 rpy_raw_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
00092 ros::names::resolve("imu") + "/rpy/raw", 5);
00093 }
00094
00095
00096
00097 int queue_size = 5;
00098
00099 imu_subscriber_.reset(new ImuSubscriber(
00100 nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
00101
00102 if (use_mag_)
00103 {
00104 if (use_magnetic_field_msg_)
00105 {
00106 mag_subscriber_.reset(new MagSubscriber(
00107 nh_, ros::names::resolve("imu") + "/mag", queue_size));
00108 }
00109 else
00110 {
00111 mag_subscriber_.reset(new MagSubscriber(
00112 nh_, ros::names::resolve("imu") + "/magnetic_field", queue_size));
00113
00114
00115
00116 mag_republisher_ = nh_.advertise<MagMsg>(
00117 ros::names::resolve("imu") + "/magnetic_field", 5);
00118 vector_mag_subscriber_.reset(new MagVectorSubscriber(
00119 nh_, ros::names::resolve("imu") + "/mag", queue_size));
00120 vector_mag_subscriber_->registerCallback(&ImuFilterRos::imuMagVectorCallback, this);
00121 }
00122
00123 sync_.reset(new Synchronizer(
00124 SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
00125 sync_->registerCallback(boost::bind(&ImuFilterRos::imuMagCallback, this, _1, _2));
00126 }
00127 else
00128 {
00129 imu_subscriber_->registerCallback(&ImuFilterRos::imuCallback, this);
00130 }
00131 }
00132
00133 ImuFilterRos::~ImuFilterRos()
00134 {
00135 ROS_INFO ("Destroying ImuFilter");
00136 }
00137
00138 void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
00139 {
00140 boost::mutex::scoped_lock(mutex_);
00141
00142 const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
00143 const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
00144
00145 ros::Time time = imu_msg_raw->header.stamp;
00146 imu_frame_ = imu_msg_raw->header.frame_id;
00147
00148 if (!initialized_)
00149 {
00150
00151 double sign = copysignf(1.0, lin_acc.z);
00152 double roll = atan2(lin_acc.y, sign * sqrt(lin_acc.x*lin_acc.x + lin_acc.z*lin_acc.z));
00153 double pitch = -atan2(lin_acc.x, sqrt(lin_acc.y*lin_acc.y + lin_acc.z*lin_acc.z));
00154 double yaw = 0.0;
00155
00156 tf2::Quaternion init_q;
00157 init_q.setRPY(roll, pitch, yaw);
00158
00159 filter_.setOrientation(init_q.getW(), init_q.getX(), init_q.getY(), init_q.getZ());
00160
00161
00162 last_time_ = time;
00163 initialized_ = true;
00164 }
00165
00166
00167 float dt;
00168 if (constant_dt_ > 0.0)
00169 dt = constant_dt_;
00170 else
00171 dt = (time - last_time_).toSec();
00172
00173 last_time_ = time;
00174
00175 filter_.madgwickAHRSupdateIMU(
00176 ang_vel.x, ang_vel.y, ang_vel.z,
00177 lin_acc.x, lin_acc.y, lin_acc.z,
00178 dt);
00179
00180 publishFilteredMsg(imu_msg_raw);
00181 if (publish_tf_)
00182 publishTransform(imu_msg_raw);
00183 }
00184
00185 void ImuFilterRos::imuMagCallback(
00186 const ImuMsg::ConstPtr& imu_msg_raw,
00187 const MagMsg::ConstPtr& mag_msg)
00188 {
00189 boost::mutex::scoped_lock(mutex_);
00190
00191 const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
00192 const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
00193 const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;
00194
00195 ros::Time time = imu_msg_raw->header.stamp;
00196 imu_frame_ = imu_msg_raw->header.frame_id;
00197
00198
00199 double mx = mag_fld.x - mag_bias_.x;
00200 double my = mag_fld.y - mag_bias_.y;
00201 double mz = mag_fld.z - mag_bias_.z;
00202
00203 float roll = 0.0;
00204 float pitch = 0.0;
00205 float yaw = 0.0;
00206
00207 if (!initialized_)
00208 {
00209
00210 if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
00211 {
00212 return;
00213 }
00214
00215 computeRPY(
00216 lin_acc.x, lin_acc.y, lin_acc.z,
00217 mx, my, mz,
00218 roll, pitch, yaw);
00219
00220 tf2::Quaternion init_q;
00221 init_q.setRPY(roll, pitch, yaw);
00222
00223 filter_.setOrientation(init_q.getW(), init_q.getX(), init_q.getY(), init_q.getZ());
00224
00225 last_time_ = time;
00226 initialized_ = true;
00227 }
00228
00229
00230 float dt;
00231 if (constant_dt_ > 0.0)
00232 dt = constant_dt_;
00233 else
00234 dt = (time - last_time_).toSec();
00235
00236 last_time_ = time;
00237
00238 filter_.madgwickAHRSupdate(
00239 ang_vel.x, ang_vel.y, ang_vel.z,
00240 lin_acc.x, lin_acc.y, lin_acc.z,
00241 mx, my, mz,
00242 dt);
00243
00244 publishFilteredMsg(imu_msg_raw);
00245 if (publish_tf_)
00246 publishTransform(imu_msg_raw);
00247
00248 if(publish_debug_topics_ && std::isfinite(mx) && std::isfinite(my) && std::isfinite(mz))
00249 {
00250 computeRPY(
00251 lin_acc.x, lin_acc.y, lin_acc.z,
00252 mx, my, mz,
00253 roll, pitch, yaw);
00254
00255 publishRawMsg(time, roll, pitch, yaw);
00256 }
00257 }
00258
00259 void ImuFilterRos::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
00260 {
00261 double q0,q1,q2,q3;
00262 filter_.getOrientation(q0,q1,q2,q3);
00263 geometry_msgs::TransformStamped transform;
00264 transform.header.stamp = imu_msg_raw->header.stamp;
00265 if (reverse_tf_)
00266 {
00267 transform.header.frame_id = imu_frame_;
00268 transform.child_frame_id = fixed_frame_;
00269 transform.transform.rotation.w = q0;
00270 transform.transform.rotation.x = -q1;
00271 transform.transform.rotation.y = -q2;
00272 transform.transform.rotation.z = -q3;
00273 }
00274 else {
00275 transform.header.frame_id = fixed_frame_;
00276 transform.child_frame_id = imu_frame_;
00277 transform.transform.rotation.w = q0;
00278 transform.transform.rotation.x = q1;
00279 transform.transform.rotation.y = q2;
00280 transform.transform.rotation.z = q3;
00281 }
00282 tf_broadcaster_.sendTransform(transform);
00283
00284 }
00285
00286 void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
00287 {
00288 double q0,q1,q2,q3;
00289 filter_.getOrientation(q0,q1,q2,q3);
00290
00291
00292 boost::shared_ptr<ImuMsg> imu_msg =
00293 boost::make_shared<ImuMsg>(*imu_msg_raw);
00294
00295 imu_msg->orientation.w = q0;
00296 imu_msg->orientation.x = q1;
00297 imu_msg->orientation.y = q2;
00298 imu_msg->orientation.z = q3;
00299
00300 imu_msg->orientation_covariance[0] = orientation_variance_;
00301 imu_msg->orientation_covariance[1] = 0.0;
00302 imu_msg->orientation_covariance[2] = 0.0;
00303 imu_msg->orientation_covariance[3] = 0.0;
00304 imu_msg->orientation_covariance[4] = orientation_variance_;
00305 imu_msg->orientation_covariance[5] = 0.0;
00306 imu_msg->orientation_covariance[6] = 0.0;
00307 imu_msg->orientation_covariance[7] = 0.0;
00308 imu_msg->orientation_covariance[8] = orientation_variance_;
00309
00310 imu_publisher_.publish(imu_msg);
00311
00312 if(publish_debug_topics_)
00313 {
00314 geometry_msgs::Vector3Stamped rpy;
00315 tf2::Matrix3x3(tf2::Quaternion(q1,q2,q3,q0)).getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
00316
00317 rpy.header = imu_msg_raw->header;
00318 rpy_filtered_debug_publisher_.publish(rpy);
00319 }
00320 }
00321
00322 void ImuFilterRos::publishRawMsg(const ros::Time& t,
00323 float roll, float pitch, float yaw)
00324 {
00325 geometry_msgs::Vector3Stamped rpy;
00326 rpy.vector.x = roll;
00327 rpy.vector.y = pitch;
00328 rpy.vector.z = yaw ;
00329 rpy.header.stamp = t;
00330 rpy.header.frame_id = imu_frame_;
00331 rpy_raw_debug_publisher_.publish(rpy);
00332 }
00333
00334 void ImuFilterRos::computeRPY(
00335 float ax, float ay, float az,
00336 float mx, float my, float mz,
00337 float& roll, float& pitch, float& yaw)
00338 {
00339
00340 double sign = copysignf(1.0, az);
00341 roll = atan2(ay, sign * sqrt(ax*ax + az*az));
00342 pitch = -atan2(ax, sqrt(ay*ay + az*az));
00343 double cos_roll = cos(roll);
00344 double sin_roll = sin(roll);
00345 double cos_pitch = cos(pitch);
00346 double sin_pitch = sin(pitch);
00347
00348
00349
00350 double head_x = mx * cos_pitch + my * sin_pitch * sin_roll + mz * sin_pitch * cos_roll;
00351 double head_y = my * cos_roll - mz * sin_roll;
00352 yaw = atan2(-head_y, head_x);
00353 }
00354
00355 void ImuFilterRos::reconfigCallback(FilterConfig& config, uint32_t level)
00356 {
00357 double gain, zeta;
00358 boost::mutex::scoped_lock(mutex_);
00359 gain = config.gain;
00360 zeta = config.zeta;
00361 filter_.setAlgorithmGain(gain);
00362 filter_.setDriftBiasGain(zeta);
00363 ROS_INFO("Imu filter gain set to %f", gain);
00364 ROS_INFO("Gyro drift bias set to %f", zeta);
00365 mag_bias_.x = config.mag_bias_x;
00366 mag_bias_.y = config.mag_bias_y;
00367 mag_bias_.z = config.mag_bias_z;
00368 orientation_variance_ = config.orientation_stddev * config.orientation_stddev;
00369 ROS_INFO("Magnetometer bias values: %f %f %f", mag_bias_.x, mag_bias_.y, mag_bias_.z);
00370 }
00371
00372 void ImuFilterRos::imuMagVectorCallback(const MagVectorMsg::ConstPtr& mag_vector_msg)
00373 {
00374 MagMsg mag_msg;
00375 mag_msg.header = mag_vector_msg->header;
00376 mag_msg.magnetic_field = mag_vector_msg->vector;
00377
00378 mag_republisher_.publish(mag_msg);
00379 }