odom_estimation_node.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Wim Meeussen */
36 
38 
39 
40 using namespace MatrixWrapper;
41 using namespace std;
42 using namespace ros;
43 using namespace tf;
44 
45 
46 static const double EPS = 1e-5;
47 
48 
49 //#define __EKF_DEBUG_FILE__
50 
51 namespace estimation
52 {
53  // constructor
54  OdomEstimationNode::OdomEstimationNode()
55  : odom_active_(false),
56  imu_active_(false),
57  vo_active_(false),
58  gps_active_(false),
59  odom_initializing_(false),
60  imu_initializing_(false),
61  vo_initializing_(false),
62  gps_initializing_(false),
63  odom_covariance_(6),
64  imu_covariance_(3),
65  vo_covariance_(6),
66  gps_covariance_(3),
67  odom_callback_counter_(0),
68  imu_callback_counter_(0),
69  vo_callback_counter_(0),
70  gps_callback_counter_(0),
71  ekf_sent_counter_(0)
72  {
73  ros::NodeHandle nh_private("~");
74  ros::NodeHandle nh;
75 
76  // paramters
77  nh_private.param("output_frame", output_frame_, std::string("odom_combined"));
78  nh_private.param("base_footprint_frame", base_footprint_frame_, std::string("base_footprint"));
79  nh_private.param("sensor_timeout", timeout_, 1.0);
80  nh_private.param("odom_used", odom_used_, true);
81  nh_private.param("imu_used", imu_used_, true);
82  nh_private.param("vo_used", vo_used_, true);
83  nh_private.param("gps_used", gps_used_, false);
84  nh_private.param("debug", debug_, false);
85  nh_private.param("self_diagnose", self_diagnose_, false);
86  double freq;
87  nh_private.param("freq", freq, 30.0);
88 
89  tf_prefix_ = tf::getPrefixParam(nh_private);
92 
93  ROS_INFO_STREAM("output frame: " << output_frame_);
94  ROS_INFO_STREAM("base frame: " << base_footprint_frame_);
95 
96  // set output frame and base frame names in OdomEstimation filter
97  // so that user-defined tf frames are respected
100 
101  timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this);
102 
103  // advertise our estimation
104  pose_pub_ = nh_private.advertise<geometry_msgs::PoseWithCovarianceStamped>("odom_combined", 10);
105 
106  // initialize
107  filter_stamp_ = Time::now();
108 
109  // subscribe to odom messages
110  if (odom_used_){
111  ROS_DEBUG("Odom sensor can be used");
112  odom_sub_ = nh.subscribe("odom", 10, &OdomEstimationNode::odomCallback, this);
113  }
114  else ROS_DEBUG("Odom sensor will NOT be used");
115 
116  // subscribe to imu messages
117  if (imu_used_){
118  ROS_DEBUG("Imu sensor can be used");
119  imu_sub_ = nh.subscribe("imu_data", 10, &OdomEstimationNode::imuCallback, this);
120  }
121  else ROS_DEBUG("Imu sensor will NOT be used");
122 
123  // subscribe to vo messages
124  if (vo_used_){
125  ROS_DEBUG("VO sensor can be used");
126  vo_sub_ = nh.subscribe("vo", 10, &OdomEstimationNode::voCallback, this);
127  }
128  else ROS_DEBUG("VO sensor will NOT be used");
129 
130  if (gps_used_){
131  ROS_DEBUG("GPS sensor can be used");
132  gps_sub_ = nh.subscribe("gps", 10, &OdomEstimationNode::gpsCallback, this);
133  }
134  else ROS_DEBUG("GPS sensor will NOT be used");
135 
136 
137  // publish state service
138  state_srv_ = nh_private.advertiseService("get_status", &OdomEstimationNode::getStatus, this);
139 
140  if (debug_){
141  // open files for debugging
142  odom_file_.open("/tmp/odom_file.txt");
143  imu_file_.open("/tmp/imu_file.txt");
144  vo_file_.open("/tmp/vo_file.txt");
145  gps_file_.open("/tmp/gps_file.txt");
146  corr_file_.open("/tmp/corr_file.txt");
147 
148 
149  }
150  };
151 
152 
153 
154 
155  // destructor
157 
158  if (debug_){
159  // close files for debugging
160  odom_file_.close();
161  imu_file_.close();
162  gps_file_.close();
163  vo_file_.close();
164  corr_file_.close();
165  }
166  };
167 
168 
169 
170 
171 
172  // callback function for odom data
174  {
176 
177  ROS_DEBUG("Odom callback at time %f ", ros::Time::now().toSec());
178  assert(odom_used_);
179 
180  // receive data
181  odom_stamp_ = odom->header.stamp;
182  odom_time_ = Time::now();
183  Quaternion q;
184  tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
185  odom_meas_ = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
186  for (unsigned int i=0; i<6; i++)
187  for (unsigned int j=0; j<6; j++)
188  odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];
189 
191 
192  // activate odom
193  if (!odom_active_) {
194  if (!odom_initializing_){
195  odom_initializing_ = true;
197  ROS_INFO("Initializing Odom sensor");
198  }
200  odom_active_ = true;
201  odom_initializing_ = false;
202  ROS_INFO("Odom sensor activated");
203  }
204  else ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.",
205  (odom_init_stamp_ - filter_stamp_).toSec());
206  }
207 
208  if (debug_){
209  // write to file
210  double tmp, yaw;
211  odom_meas_.getBasis().getEulerYPR(yaw, tmp, tmp);
212  odom_file_<< fixed <<setprecision(5) << ros::Time::now().toSec() << " " << odom_meas_.getOrigin().x() << " " << odom_meas_.getOrigin().y() << " " << yaw << " " << endl;
213  }
214  };
215 
216 
217 
218 
219  // callback function for imu data
221  {
223 
224  assert(imu_used_);
225 
226  // receive data
227  imu_stamp_ = imu->header.stamp;
228  tf::Quaternion orientation;
229  quaternionMsgToTF(imu->orientation, orientation);
230  imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
231  for (unsigned int i=0; i<3; i++)
232  for (unsigned int j=0; j<3; j++)
233  imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
234 
235  // Transforms imu data to base_footprint frame
236  if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
237  // warn when imu was already activated, not when imu is not active yet
238  if (imu_active_)
239  ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
240  else if (my_filter_.isInitialized())
241  ROS_WARN("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
242  else
243  ROS_DEBUG("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
244  return;
245  }
246  StampedTransform base_imu_offset;
247  robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset);
248  imu_meas_ = imu_meas_ * base_imu_offset;
249 
250  imu_time_ = Time::now();
251 
252  // manually set covariance untile imu sends covariance
253  if (imu_covariance_(1,1) == 0.0){
254  SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
255  measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
256  measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
257  measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
258  imu_covariance_ = measNoiseImu_Cov;
259  }
260 
262 
263  // activate imu
264  if (!imu_active_) {
265  if (!imu_initializing_){
266  imu_initializing_ = true;
268  ROS_INFO("Initializing Imu sensor");
269  }
271  imu_active_ = true;
272  imu_initializing_ = false;
273  ROS_INFO("Imu sensor activated");
274  }
275  else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.",
276  (imu_init_stamp_ - filter_stamp_).toSec());
277  }
278 
279  if (debug_){
280  // write to file
281  double tmp, yaw;
282  imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp);
283  imu_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< yaw << endl;
284  }
285  };
286 
287 
288 
289 
290  // callback function for VO data
292  {
294 
295  assert(vo_used_);
296 
297  // get data
298  vo_stamp_ = vo->header.stamp;
299  vo_time_ = Time::now();
300  poseMsgToTF(vo->pose.pose, vo_meas_);
301  for (unsigned int i=0; i<6; i++)
302  for (unsigned int j=0; j<6; j++)
303  vo_covariance_(i+1, j+1) = vo->pose.covariance[6*i+j];
305 
306  // activate vo
307  if (!vo_active_) {
308  if (!vo_initializing_){
309  vo_initializing_ = true;
311  ROS_INFO("Initializing Vo sensor");
312  }
314  vo_active_ = true;
315  vo_initializing_ = false;
316  ROS_INFO("Vo sensor activated");
317  }
318  else ROS_DEBUG("Waiting to activate VO, because VO measurements are still %f sec in the future.",
319  (vo_init_stamp_ - filter_stamp_).toSec());
320  }
321 
322  if (debug_){
323  // write to file
324  double Rx, Ry, Rz;
325  vo_meas_.getBasis().getEulerYPR(Rz, Ry, Rx);
326  vo_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< vo_meas_.getOrigin().x() << " " << vo_meas_.getOrigin().y() << " " << vo_meas_.getOrigin().z() << " "
327  << Rx << " " << Ry << " " << Rz << endl;
328  }
329  };
330 
331 
333  {
335 
336  assert(gps_used_);
337 
338  // get data
339  gps_stamp_ = gps->header.stamp;
340  gps_time_ = Time::now();
341  geometry_msgs::PoseWithCovariance gps_pose = gps->pose;
342  if (isnan(gps_pose.pose.position.z)){
343  // if we have no linear z component in the GPS message, set it to 0 so that we can still get a transform via `tf
344  // (which does not like "NaN" values)
345  gps_pose.pose.position.z = 0;
346  // set the covariance for the linear z component very high so we just ignore it
347  gps_pose.covariance[6*2 + 2] = std::numeric_limits<double>::max();
348  }
349  poseMsgToTF(gps_pose.pose, gps_meas_);
350  for (unsigned int i=0; i<3; i++)
351  for (unsigned int j=0; j<3; j++)
352  gps_covariance_(i+1, j+1) = gps_pose.covariance[6*i+j];
354 
355  // activate gps
356  if (!gps_active_) {
357  if (!gps_initializing_){
358  gps_initializing_ = true;
360  ROS_INFO("Initializing GPS sensor");
361  }
363  gps_active_ = true;
364  gps_initializing_ = false;
365  ROS_INFO("GPS sensor activated");
366  }
367  else ROS_DEBUG("Waiting to activate GPS, because GPS measurements are still %f sec in the future.",
368  (gps_init_stamp_ - filter_stamp_).toSec());
369  }
370 
371  if (debug_){
372  // write to file
373  gps_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< gps_meas_.getOrigin().x() << " " << gps_meas_.getOrigin().y() << " " << gps_meas_.getOrigin().z() <<endl;
374  }
375  };
376 
377 
378 
379  // filter loop
381  {
382  ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec());
383 
384  // check for timing problems
386  double diff = fabs( Duration(odom_stamp_ - imu_stamp_).toSec() );
387  if (diff > 1.0) ROS_ERROR("Timestamps of odometry and imu are %f seconds apart.", diff);
388  }
389 
390  // initial value for filter stamp; keep this stamp when no sensors are active
391  filter_stamp_ = Time::now();
392 
393  // check which sensors are still active
394  if ((odom_active_ || odom_initializing_) &&
395  (Time::now() - odom_time_).toSec() > timeout_){
396  odom_active_ = false; odom_initializing_ = false;
397  ROS_INFO("Odom sensor not active any more");
398  }
399  if ((imu_active_ || imu_initializing_) &&
400  (Time::now() - imu_time_).toSec() > timeout_){
401  imu_active_ = false; imu_initializing_ = false;
402  ROS_INFO("Imu sensor not active any more");
403  }
404  if ((vo_active_ || vo_initializing_) &&
405  (Time::now() - vo_time_).toSec() > timeout_){
406  vo_active_ = false; vo_initializing_ = false;
407  ROS_INFO("VO sensor not active any more");
408  }
409 
410  if ((gps_active_ || gps_initializing_) &&
411  (Time::now() - gps_time_).toSec() > timeout_){
412  gps_active_ = false; gps_initializing_ = false;
413  ROS_INFO("GPS sensor not active any more");
414  }
415 
416 
417  // only update filter when one of the sensors is active
419 
420  // update filter at time where all sensor measurements are available
425 
426 
427  // update filter
428  if ( my_filter_.isInitialized() ) {
429  bool diagnostics = true;
431 
432  // output most recent estimate and relative covariance
436 
437  // broadcast most recent estimate to TransformArray
438  StampedTransform tmp;
440  if(!vo_active_ && !gps_active_)
441  tmp.getOrigin().setZ(0.0);
443 
444  if (debug_){
445  // write to file
446  ColumnVector estimate;
447  my_filter_.getEstimate(estimate);
448  corr_file_ << fixed << setprecision(5)<<ros::Time::now().toSec()<<" ";
449 
450  for (unsigned int i=1; i<=6; i++)
451  corr_file_ << estimate(i) << " ";
452  corr_file_ << endl;
453  }
454  }
455  if (self_diagnose_ && !diagnostics)
456  ROS_WARN("Robot pose ekf diagnostics discovered a potential problem");
457  }
458 
459 
460  // initialize filer with odometry frame
464  Transform init_meas_ = Transform(q, p);
465  my_filter_.initialize(init_meas_, gps_stamp_);
466  ROS_INFO("Kalman filter initialized with gps and imu measurement");
467  }
468  else if ( odom_active_ && gps_active_ && !my_filter_.isInitialized()) {
471  Transform init_meas_ = Transform(q, p);
472  my_filter_.initialize(init_meas_, gps_stamp_);
473  ROS_INFO("Kalman filter initialized with gps and odometry measurement");
474  }
475  else if ( vo_active_ && gps_active_ && !my_filter_.isInitialized()) {
478  Transform init_meas_ = Transform(q, p);
479  my_filter_.initialize(init_meas_, gps_stamp_);
480  ROS_INFO("Kalman filter initialized with gps and visual odometry measurement");
481  }
482  else if ( odom_active_ && !gps_used_ && !my_filter_.isInitialized()){
484  ROS_INFO("Kalman filter initialized with odom measurement");
485  }
486  else if ( vo_active_ && !gps_used_ && !my_filter_.isInitialized()){
488  ROS_INFO("Kalman filter initialized with vo measurement");
489  }
490  }
491  };
492 
493 
494 bool OdomEstimationNode::getStatus(robot_pose_ekf::GetStatus::Request& req, robot_pose_ekf::GetStatus::Response& resp)
495 {
496  stringstream ss;
497  ss << "Input:" << endl;
498  ss << " * Odometry sensor" << endl;
499  ss << " - is "; if (!odom_used_) ss << "NOT "; ss << "used" << endl;
500  ss << " - is "; if (!odom_active_) ss << "NOT "; ss << "active" << endl;
501  ss << " - received " << odom_callback_counter_ << " messages" << endl;
502  ss << " - listens to topic " << odom_sub_.getTopic() << endl;
503  ss << " * IMU sensor" << endl;
504  ss << " - is "; if (!imu_used_) ss << "NOT "; ss << "used" << endl;
505  ss << " - is "; if (!imu_active_) ss << "NOT "; ss << "active" << endl;
506  ss << " - received " << imu_callback_counter_ << " messages" << endl;
507  ss << " - listens to topic " << imu_sub_.getTopic() << endl;
508  ss << " * Visual Odometry sensor" << endl;
509  ss << " - is "; if (!vo_used_) ss << "NOT "; ss << "used" << endl;
510  ss << " - is "; if (!vo_active_) ss << "NOT "; ss << "active" << endl;
511  ss << " - received " << vo_callback_counter_ << " messages" << endl;
512  ss << " - listens to topic " << vo_sub_.getTopic() << endl;
513  ss << " * GPS sensor" << endl;
514  ss << " - is "; if (!gps_used_) ss << "NOT "; ss << "used" << endl;
515  ss << " - is "; if (!gps_active_) ss << "NOT "; ss << "active" << endl;
516  ss << " - received " << gps_callback_counter_ << " messages" << endl;
517  ss << " - listens to topic " << gps_sub_.getTopic() << endl;
518  ss << "Output:" << endl;
519  ss << " * Robot pose ekf filter" << endl;
520  ss << " - is "; if (!my_filter_.isInitialized()) ss << "NOT "; ss << "active" << endl;
521  ss << " - sent " << ekf_sent_counter_ << " messages" << endl;
522  ss << " - pulishes on topics " << pose_pub_.getTopic() << " and /tf" << endl;
523  resp.status = ss.str();
524  return true;
525 }
526 
527 }; // namespace
528 
529 
530 
531 
532 
533 
534 // ----------
535 // -- MAIN --
536 // ----------
537 using namespace estimation;
538 int main(int argc, char **argv)
539 {
540  // Initialize ROS
541  ros::init(argc, argv, "robot_pose_ekf");
542 
543  // create filter class
544  OdomEstimationNode my_filter_node;
545 
546  ros::spin();
547 
548  return 0;
549 }
void spin(const ros::TimerEvent &e)
the mail filter loop that will be called periodically
std::string getTopic() const
resp
Definition: wtf.py:12
std::string getPrefixParam(ros::NodeHandle &nh)
Transform inverse() const
Quaternion getRotation() const
MatrixWrapper::SymmetricMatrix odom_covariance_
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
bool getStatus(robot_pose_ekf::GetStatus::Request &req, robot_pose_ekf::GetStatus::Response &resp)
get the status of the filter
void initialize(const tf::Transform &prior, const ros::Time &time)
static const double EPS
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
geometry_msgs::PoseWithCovarianceStamped output_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) const
void odomCallback(const OdomConstPtr &odom)
callback function for odo data
MatrixWrapper::SymmetricMatrix vo_covariance_
#define ROS_WARN(...)
tf::TransformBroadcaster odom_broadcaster_
void voCallback(const VoConstPtr &vo)
callback function for vo data
std::string resolve(const std::string &prefix, const std::string &frame_name)
void imuCallback(const ImuConstPtr &imu)
callback function for imu data
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
MatrixWrapper::SymmetricMatrix imu_covariance_
#define ROS_INFO(...)
void sendTransform(const StampedTransform &transform)
bool update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const ros::Time &filter_time, bool &diagnostics_res)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
void addMeasurement(const tf::StampedTransform &meas)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
std::string getTopic() const
#define ROS_INFO_STREAM(args)
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
int min(int a, int b)
static Time now()
void setBaseFootprintFrame(const std::string &base_frame)
MatrixWrapper::SymmetricMatrix gps_covariance_
void gpsCallback(const GpsConstPtr &gps)
callback function for vo data
void setOutputFrame(const std::string &output_frame)
#define ROS_ERROR(...)
void getEstimate(MatrixWrapper::ColumnVector &estimate)
int main(int argc, char **argv)
#define ROS_DEBUG(...)


robot_pose_ekf
Author(s): Wim Meeussen
autogenerated on Mon Feb 28 2022 23:26:03