imu_filter_ros.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010, CCNY Robotics Lab
3  * Ivan Dryanovski <ivan.dryanovski@gmail.com>
4  *
5  * http://robotics.ccny.cuny.edu
6  *
7  * Based on implementation of Madgwick's IMU and AHRS algorithms.
8  * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
9  *
10  *
11  * This program is free software: you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation, either version 3 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program. If not, see <http://www.gnu.org/licenses/>.
23  */
24 
27 #include "geometry_msgs/TransformStamped.h"
30 
32  nh_(nh),
33  nh_private_(nh_private),
34  initialized_(false)
35 {
36  ROS_INFO ("Starting ImuFilter");
37 
38  // **** get paramters
39  if (!nh_private_.getParam ("stateless", stateless_))
40  stateless_ = false;
41  if (!nh_private_.getParam ("use_mag", use_mag_))
42  use_mag_ = true;
43  if (!nh_private_.getParam ("publish_tf", publish_tf_))
44  publish_tf_ = true;
45  if (!nh_private_.getParam ("reverse_tf", reverse_tf_))
46  reverse_tf_ = false;
47  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
48  fixed_frame_ = "odom";
49  if (!nh_private_.getParam ("constant_dt", constant_dt_))
50  constant_dt_ = 0.0;
51  if (!nh_private_.getParam ("remove_gravity_vector", remove_gravity_vector_))
53  if (!nh_private_.getParam ("publish_debug_topics", publish_debug_topics_))
54  publish_debug_topics_= false;
55  if (!nh_private_.getParam ("use_magnetic_field_msg", use_magnetic_field_msg_))
57 
58  std::string world_frame;
59  // Default should become false for next release
60  if (!nh_private_.getParam ("world_frame", world_frame)) {
61  world_frame = "nwu";
62  ROS_WARN("Deprecation Warning: The parameter world_frame was not set, default is 'nwu'.");
63  ROS_WARN("Starting with ROS Lunar, world_frame will default to 'enu'!");
64  }
65 
66  if (world_frame == "ned") {
68  } else if (world_frame == "nwu"){
70  } else if (world_frame == "enu"){
72  } else {
73  ROS_ERROR("The parameter world_frame was set to invalid value '%s'.", world_frame.c_str());
74  ROS_ERROR("Valid values are 'enu', 'ned' and 'nwu'. Setting to 'enu'.");
76  }
78 
79  // check for illegal constant_dt values
80  if (constant_dt_ < 0.0)
81  {
82  ROS_FATAL("constant_dt parameter is %f, must be >= 0.0. Setting to 0.0", constant_dt_);
83  constant_dt_ = 0.0;
84  }
85 
86  // if constant_dt_ is 0.0 (default), use IMU timestamp to determine dt
87  // otherwise, it will be constant
88  if (constant_dt_ == 0.0)
89  ROS_INFO("Using dt computed from message headers");
90  else
91  ROS_INFO("Using constant dt of %f sec", constant_dt_);
92 
94  ROS_INFO("The gravity vector will be removed from the acceleration");
95  else
96  ROS_INFO("The gravity vector is kept in the IMU message.");
97 
98  // **** register dynamic reconfigure
100  FilterConfigServer::CallbackType f = boost::bind(&ImuFilterRos::reconfigCallback, this, _1, _2);
101  config_server_->setCallback(f);
102 
103  // **** register publishers
104  imu_publisher_ = nh_.advertise<sensor_msgs::Imu>(
105  ros::names::resolve("imu") + "/data", 5);
106 
108  {
109  rpy_filtered_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
110  ros::names::resolve("imu") + "/rpy/filtered", 5);
111 
112  rpy_raw_debug_publisher_ = nh_.advertise<geometry_msgs::Vector3Stamped>(
113  ros::names::resolve("imu") + "/rpy/raw", 5);
114  }
115 
116  // **** register subscribers
117  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
118  int queue_size = 5;
119 
120  imu_subscriber_.reset(new ImuSubscriber(
121  nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
122 
123  if (use_mag_)
124  {
126  {
127  mag_subscriber_.reset(new MagSubscriber(
128  nh_, ros::names::resolve("imu") + "/mag", queue_size));
129  }
130  else
131  {
132  mag_subscriber_.reset(new MagSubscriber(
133  nh_, ros::names::resolve("imu") + "/magnetic_field", queue_size));
134 
135  // Initialize the shim to support republishing Vector3Stamped messages from /mag as MagneticField
136  // messages on the /magnetic_field topic.
138  ros::names::resolve("imu") + "/magnetic_field", 5);
140  nh_, ros::names::resolve("imu") + "/mag", queue_size));
142  }
143 
144  sync_.reset(new Synchronizer(
145  SyncPolicy(queue_size), *imu_subscriber_, *mag_subscriber_));
146  sync_->registerCallback(boost::bind(&ImuFilterRos::imuMagCallback, this, _1, _2));
147  }
148  else
149  {
150  imu_subscriber_->registerCallback(&ImuFilterRos::imuCallback, this);
151  }
152 
154 }
155 
157 {
158  ROS_INFO ("Destroying ImuFilter");
159 
160  // Explicitly stop callbacks; they could execute after we're destroyed
162 }
163 
164 void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
165 {
166  boost::mutex::scoped_lock lock(mutex_);
167 
168  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
169  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
170 
171  ros::Time time = imu_msg_raw->header.stamp;
172  imu_frame_ = imu_msg_raw->header.frame_id;
173 
174  if (!initialized_ || stateless_)
175  {
176  geometry_msgs::Quaternion init_q;
178  {
179  ROS_WARN_THROTTLE(5.0, "The IMU seems to be in free fall, cannot determine gravity direction!");
180  return;
181  }
182  filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
183  }
184 
185  if (!initialized_)
186  {
187  ROS_INFO("First IMU message received.");
189 
190  // initialize time
191  last_time_ = time;
192  initialized_ = true;
193  }
194 
195  // determine dt: either constant, or from IMU timestamp
196  float dt;
197  if (constant_dt_ > 0.0)
198  dt = constant_dt_;
199  else
200  {
201  dt = (time - last_time_).toSec();
202  if (time.isZero())
203  ROS_WARN_STREAM_THROTTLE(5.0, "The IMU message time stamp is zero, and the parameter constant_dt is not set!" <<
204  " The filter will not update the orientation.");
205  }
206 
207  last_time_ = time;
208 
209  if (!stateless_)
211  ang_vel.x, ang_vel.y, ang_vel.z,
212  lin_acc.x, lin_acc.y, lin_acc.z,
213  dt);
214 
215  publishFilteredMsg(imu_msg_raw);
216  if (publish_tf_)
217  publishTransform(imu_msg_raw);
218 }
219 
221  const ImuMsg::ConstPtr& imu_msg_raw,
222  const MagMsg::ConstPtr& mag_msg)
223 {
224  boost::mutex::scoped_lock lock(mutex_);
225 
226  const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
227  const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
228  const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;
229 
230  ros::Time time = imu_msg_raw->header.stamp;
231  imu_frame_ = imu_msg_raw->header.frame_id;
232 
233  /*** Compensate for hard iron ***/
234  geometry_msgs::Vector3 mag_compensated;
235  mag_compensated.x = mag_fld.x - mag_bias_.x;
236  mag_compensated.y = mag_fld.y - mag_bias_.y;
237  mag_compensated.z = mag_fld.z - mag_bias_.z;
238 
239  double roll = 0.0;
240  double pitch = 0.0;
241  double yaw = 0.0;
242 
243  if (!initialized_ || stateless_)
244  {
245  // wait for mag message without NaN / inf
246  if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
247  {
248  return;
249  }
250 
251  geometry_msgs::Quaternion init_q;
252  if (!StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, init_q))
253  {
254  ROS_WARN_THROTTLE(5.0, "The IMU seems to be in free fall or close to magnetic north pole, cannot determine gravity direction!");
255  return;
256  }
257  filter_.setOrientation(init_q.w, init_q.x, init_q.y, init_q.z);
258  }
259 
260  if (!initialized_)
261  {
262  ROS_INFO("First pair of IMU and magnetometer messages received.");
264 
265  // initialize time
266  last_time_ = time;
267  initialized_ = true;
268  }
269 
270  // determine dt: either constant, or from IMU timestamp
271  float dt;
272  if (constant_dt_ > 0.0)
273  dt = constant_dt_;
274  else
275  {
276  dt = (time - last_time_).toSec();
277  if (time.isZero())
278  ROS_WARN_STREAM_THROTTLE(5.0, "The IMU message time stamp is zero, and the parameter constant_dt is not set!" <<
279  " The filter will not update the orientation.");
280  }
281 
282  last_time_ = time;
283 
284  if (!stateless_)
286  ang_vel.x, ang_vel.y, ang_vel.z,
287  lin_acc.x, lin_acc.y, lin_acc.z,
288  mag_compensated.x, mag_compensated.y, mag_compensated.z,
289  dt);
290 
291  publishFilteredMsg(imu_msg_raw);
292  if (publish_tf_)
293  publishTransform(imu_msg_raw);
294 
296  {
297  geometry_msgs::Quaternion orientation;
298  if (StatelessOrientation::computeOrientation(world_frame_, lin_acc, mag_compensated, orientation))
299  {
300  tf2::Matrix3x3(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)).getRPY(roll, pitch, yaw, 0);
301  publishRawMsg(time, roll, pitch, yaw);
302  }
303  }
304 }
305 
306 void ImuFilterRos::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
307 {
308  double q0,q1,q2,q3;
309  filter_.getOrientation(q0,q1,q2,q3);
310  geometry_msgs::TransformStamped transform;
311  transform.header.stamp = imu_msg_raw->header.stamp;
312  if (reverse_tf_)
313  {
314  transform.header.frame_id = imu_frame_;
315  transform.child_frame_id = fixed_frame_;
316  transform.transform.rotation.w = q0;
317  transform.transform.rotation.x = -q1;
318  transform.transform.rotation.y = -q2;
319  transform.transform.rotation.z = -q3;
320  }
321  else {
322  transform.header.frame_id = fixed_frame_;
323  transform.child_frame_id = imu_frame_;
324  transform.transform.rotation.w = q0;
325  transform.transform.rotation.x = q1;
326  transform.transform.rotation.y = q2;
327  transform.transform.rotation.z = q3;
328  }
329  tf_broadcaster_.sendTransform(transform);
330 
331 }
332 
333 void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw)
334 {
335  double q0,q1,q2,q3;
336  filter_.getOrientation(q0,q1,q2,q3);
337 
338  // create and publish filtered IMU message
339  boost::shared_ptr<ImuMsg> imu_msg =
340  boost::make_shared<ImuMsg>(*imu_msg_raw);
341 
342  imu_msg->orientation.w = q0;
343  imu_msg->orientation.x = q1;
344  imu_msg->orientation.y = q2;
345  imu_msg->orientation.z = q3;
346 
347  imu_msg->orientation_covariance[0] = orientation_variance_;
348  imu_msg->orientation_covariance[1] = 0.0;
349  imu_msg->orientation_covariance[2] = 0.0;
350  imu_msg->orientation_covariance[3] = 0.0;
351  imu_msg->orientation_covariance[4] = orientation_variance_;
352  imu_msg->orientation_covariance[5] = 0.0;
353  imu_msg->orientation_covariance[6] = 0.0;
354  imu_msg->orientation_covariance[7] = 0.0;
355  imu_msg->orientation_covariance[8] = orientation_variance_;
356 
357 
359  float gx, gy, gz;
360  filter_.getGravity(gx, gy, gz);
361  imu_msg->linear_acceleration.x -= gx;
362  imu_msg->linear_acceleration.y -= gy;
363  imu_msg->linear_acceleration.z -= gz;
364  }
365 
366  imu_publisher_.publish(imu_msg);
367 
369  {
370  geometry_msgs::Vector3Stamped rpy;
371  tf2::Matrix3x3(tf2::Quaternion(q1,q2,q3,q0)).getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
372 
373  rpy.header = imu_msg_raw->header;
375  }
376 }
377 
379  float roll, float pitch, float yaw)
380 {
381  geometry_msgs::Vector3Stamped rpy;
382  rpy.vector.x = roll;
383  rpy.vector.y = pitch;
384  rpy.vector.z = yaw ;
385  rpy.header.stamp = t;
386  rpy.header.frame_id = imu_frame_;
388 }
389 
390 
391 void ImuFilterRos::reconfigCallback(FilterConfig& config, uint32_t level)
392 {
393  double gain, zeta;
394  boost::mutex::scoped_lock lock(mutex_);
395  gain = config.gain;
396  zeta = config.zeta;
399  ROS_INFO("Imu filter gain set to %f", gain);
400  ROS_INFO("Gyro drift bias set to %f", zeta);
401  mag_bias_.x = config.mag_bias_x;
402  mag_bias_.y = config.mag_bias_y;
403  mag_bias_.z = config.mag_bias_z;
404  orientation_variance_ = config.orientation_stddev * config.orientation_stddev;
405  ROS_INFO("Magnetometer bias values: %f %f %f", mag_bias_.x, mag_bias_.y, mag_bias_.z);
406 }
407 
408 void ImuFilterRos::imuMagVectorCallback(const MagVectorMsg::ConstPtr& mag_vector_msg)
409 {
410  MagMsg mag_msg;
411  mag_msg.header = mag_vector_msg->header;
412  mag_msg.magnetic_field = mag_vector_msg->vector;
413  // leaving mag_msg.magnetic_field_covariance set to all zeros (= "covariance unknown")
414  mag_republisher_.publish(mag_msg);
415 }
416 
418 {
419  if (use_mag_)
420  ROS_WARN_STREAM("Still waiting for data on topics " << ros::names::resolve("imu") << "/data_raw"
421  << " and " << ros::names::resolve("imu") << "/mag" << "...");
422  else
423  ROS_WARN_STREAM("Still waiting for data on topic " << ros::names::resolve("imu") << "/data_raw" << "...");
424 }
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
boost::shared_ptr< MagVectorSubscriber > vector_mag_subscriber_
#define ROS_FATAL(...)
#define ROS_WARN_THROTTLE(rate,...)
message_filters::Subscriber< ImuMsg > ImuSubscriber
void publish(const boost::shared_ptr< M > &message) const
message_filters::sync_policies::ApproximateTime< ImuMsg, MagMsg > SyncPolicy
f
bool publish_debug_topics_
boost::shared_ptr< Synchronizer > sync_
double orientation_variance_
void publishRawMsg(const ros::Time &t, float roll, float pitch, float yaw)
void imuMagVectorCallback(const MagVectorMsg::ConstPtr &mag_vector_msg)
void imuMagCallback(const ImuMsg::ConstPtr &imu_msg_raw, const MagMsg::ConstPtr &mav_msg)
void setOrientation(double q0, double q1, double q2, double q3)
Definition: imu_filter.h:82
void stop()
ros::Publisher rpy_raw_debug_publisher_
void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
Definition: imu_filter.cpp:175
void getGravity(float &rx, float &ry, float &rz, float gravity=9.80665)
Definition: imu_filter.cpp:313
imu_filter_madgwick::ImuFilterMadgwickConfig FilterConfig
bool use_magnetic_field_msg_
double constant_dt_
message_filters::Synchronizer< SyncPolicy > Synchronizer
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
std::string imu_frame_
message_filters::Subscriber< MagMsg > MagSubscriber
ros::Time last_time_
void checkTopicsTimerCallback(const ros::TimerEvent &)
#define ROS_WARN(...)
void getOrientation(double &q0, double &q1, double &q2, double &q3)
Definition: imu_filter.h:65
boost::shared_ptr< FilterConfigServer > config_server_
std::string fixed_frame_
ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private)
void setAlgorithmGain(double gain)
Definition: imu_filter.h:50
ros::NodeHandle nh_private_
boost::shared_ptr< ImuSubscriber > imu_subscriber_
sensor_msgs::MagneticField MagMsg
bool remove_gravity_vector_
void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt)
Definition: imu_filter.cpp:258
message_filters::Subscriber< MagVectorMsg > MagVectorSubscriber
ros::Publisher imu_publisher_
#define ROS_INFO(...)
dynamic_reconfigure::Server< FilterConfig > FilterConfigServer
void setWorldFrame(WorldFrame::WorldFrame frame)
Definition: imu_filter.h:60
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
boost::mutex mutex_
ros::Publisher mag_republisher_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual ~ImuFilterRos()
void publishTransform(const ImuMsg::ConstPtr &imu_msg_raw)
#define ROS_WARN_STREAM(args)
void sendTransform(const geometry_msgs::TransformStamped &transform)
ros::NodeHandle nh_
#define ROS_WARN_STREAM_THROTTLE(rate, args)
ImuFilter filter_
ros::Timer check_topics_timer_
static bool computeOrientation(WorldFrame::WorldFrame frame, geometry_msgs::Vector3 acceleration, geometry_msgs::Vector3 magneticField, geometry_msgs::Quaternion &orientation)
void setDriftBiasGain(double zeta)
Definition: imu_filter.h:55
void reconfigCallback(FilterConfig &config, uint32_t level)
bool getParam(const std::string &key, std::string &s) const
void publishFilteredMsg(const ImuMsg::ConstPtr &imu_msg_raw)
WorldFrame::WorldFrame world_frame_
tf2_ros::TransformBroadcaster tf_broadcaster_
void imuCallback(const ImuMsg::ConstPtr &imu_msg_raw)
boost::shared_ptr< MagSubscriber > mag_subscriber_
ros::Publisher rpy_filtered_debug_publisher_
#define ROS_ERROR(...)
geometry_msgs::Vector3 mag_bias_


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Thu Jun 18 2020 03:40:00