odom_estimation_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Wim Meeussen */
00036 
00037 #include <robot_pose_ekf/odom_estimation_node.h>
00038 
00039 
00040 using namespace MatrixWrapper;
00041 using namespace std;
00042 using namespace ros;
00043 using namespace tf;
00044 
00045 
00046 static const double EPS = 1e-5;
00047 
00048 
00049 //#define __EKF_DEBUG_FILE__
00050 
00051 namespace estimation
00052 {
00053   // constructor
00054   OdomEstimationNode::OdomEstimationNode()
00055     : odom_active_(false),
00056       imu_active_(false),
00057       vo_active_(false),
00058       gps_active_(false),
00059       odom_initializing_(false),
00060       imu_initializing_(false),
00061       vo_initializing_(false),
00062       gps_initializing_(false),
00063       odom_covariance_(6),
00064       imu_covariance_(3),
00065       vo_covariance_(6),
00066       gps_covariance_(3),
00067       odom_callback_counter_(0),
00068       imu_callback_counter_(0),
00069       vo_callback_counter_(0),
00070       gps_callback_counter_(0),
00071       ekf_sent_counter_(0)
00072   {
00073     ros::NodeHandle nh_private("~");
00074     ros::NodeHandle nh;
00075 
00076     // paramters
00077     nh_private.param("output_frame", output_frame_, std::string("odom_combined"));
00078     nh_private.param("base_footprint_frame", base_footprint_frame_, std::string("base_footprint"));
00079     nh_private.param("sensor_timeout", timeout_, 1.0);
00080     nh_private.param("odom_used", odom_used_, true);
00081     nh_private.param("imu_used",  imu_used_, true);
00082     nh_private.param("vo_used",   vo_used_, true);
00083     nh_private.param("gps_used",   gps_used_, false);
00084     nh_private.param("debug",   debug_, false);
00085     nh_private.param("self_diagnose",  self_diagnose_, false);
00086     double freq;
00087     nh_private.param("freq", freq, 30.0);
00088 
00089     tf_prefix_ = tf::getPrefixParam(nh_private);
00090     output_frame_ = tf::resolve(tf_prefix_, output_frame_);
00091     base_footprint_frame_ = tf::resolve(tf_prefix_, base_footprint_frame_);
00092 
00093     ROS_INFO_STREAM("output frame: " << output_frame_);
00094     ROS_INFO_STREAM("base frame: " << base_footprint_frame_);
00095 
00096     // set output frame and base frame names in OdomEstimation filter
00097     // so that user-defined tf frames are respected
00098     my_filter_.setOutputFrame(output_frame_);
00099     my_filter_.setBaseFootprintFrame(base_footprint_frame_);
00100 
00101     timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this);
00102 
00103     // advertise our estimation
00104     pose_pub_ = nh_private.advertise<geometry_msgs::PoseWithCovarianceStamped>("odom_combined", 10);
00105 
00106     // initialize
00107     filter_stamp_ = Time::now();
00108 
00109     // subscribe to odom messages
00110     if (odom_used_){
00111       ROS_DEBUG("Odom sensor can be used");
00112       odom_sub_ = nh.subscribe("odom", 10, &OdomEstimationNode::odomCallback, this);
00113     }
00114     else ROS_DEBUG("Odom sensor will NOT be used");
00115 
00116     // subscribe to imu messages
00117     if (imu_used_){
00118       ROS_DEBUG("Imu sensor can be used");
00119       imu_sub_ = nh.subscribe("imu_data", 10,  &OdomEstimationNode::imuCallback, this);
00120     }
00121     else ROS_DEBUG("Imu sensor will NOT be used");
00122 
00123     // subscribe to vo messages
00124     if (vo_used_){
00125       ROS_DEBUG("VO sensor can be used");
00126       vo_sub_ = nh.subscribe("vo", 10, &OdomEstimationNode::voCallback, this);
00127     }
00128     else ROS_DEBUG("VO sensor will NOT be used");
00129 
00130     if (gps_used_){
00131       ROS_DEBUG("GPS sensor can be used");
00132       gps_sub_ = nh.subscribe("gps", 10, &OdomEstimationNode::gpsCallback, this);
00133     }
00134     else ROS_DEBUG("GPS sensor will NOT be used");
00135 
00136 
00137     // publish state service
00138     state_srv_ = nh_private.advertiseService("get_status", &OdomEstimationNode::getStatus, this);
00139 
00140     if (debug_){
00141       // open files for debugging
00142       odom_file_.open("/tmp/odom_file.txt");
00143       imu_file_.open("/tmp/imu_file.txt");
00144       vo_file_.open("/tmp/vo_file.txt");
00145       gps_file_.open("/tmp/gps_file.txt");
00146       corr_file_.open("/tmp/corr_file.txt");
00147 
00148   
00149     }
00150   };
00151 
00152 
00153 
00154 
00155   // destructor
00156   OdomEstimationNode::~OdomEstimationNode(){
00157 
00158     if (debug_){
00159       // close files for debugging
00160       odom_file_.close();
00161       imu_file_.close();
00162       gps_file_.close();
00163       vo_file_.close();
00164       corr_file_.close();
00165     }
00166   };
00167 
00168 
00169 
00170 
00171 
00172   // callback function for odom data
00173   void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)
00174   {
00175     odom_callback_counter_++;
00176 
00177     ROS_DEBUG("Odom callback at time %f ", ros::Time::now().toSec());
00178     assert(odom_used_);
00179 
00180     // receive data 
00181     odom_stamp_ = odom->header.stamp;
00182     odom_time_  = Time::now();
00183     Quaternion q;
00184     tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
00185     odom_meas_  = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
00186     for (unsigned int i=0; i<6; i++)
00187       for (unsigned int j=0; j<6; j++)
00188         odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];
00189 
00190     my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);
00191     
00192     // activate odom
00193     if (!odom_active_) {
00194       if (!odom_initializing_){
00195         odom_initializing_ = true;
00196         odom_init_stamp_ = odom_stamp_;
00197         ROS_INFO("Initializing Odom sensor");      
00198       }
00199       if ( filter_stamp_ >= odom_init_stamp_){
00200         odom_active_ = true;
00201         odom_initializing_ = false;
00202         ROS_INFO("Odom sensor activated");      
00203       }
00204       else ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.", 
00205                     (odom_init_stamp_ - filter_stamp_).toSec());
00206     }
00207     
00208     if (debug_){
00209       // write to file
00210       double tmp, yaw;
00211       odom_meas_.getBasis().getEulerYPR(yaw, tmp, tmp);
00212       odom_file_<< fixed <<setprecision(5) << ros::Time::now().toSec() << " " << odom_meas_.getOrigin().x() << " " << odom_meas_.getOrigin().y() << "  " << yaw << "  " << endl;
00213     }
00214   };
00215 
00216 
00217 
00218 
00219   // callback function for imu data
00220   void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
00221   {
00222     imu_callback_counter_++;
00223 
00224     assert(imu_used_);
00225 
00226     // receive data 
00227     imu_stamp_ = imu->header.stamp;
00228     tf::Quaternion orientation;
00229     quaternionMsgToTF(imu->orientation, orientation);
00230     imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
00231     for (unsigned int i=0; i<3; i++)
00232       for (unsigned int j=0; j<3; j++)
00233         imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
00234 
00235     // Transforms imu data to base_footprint frame
00236     if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
00237       // warn when imu was already activated, not when imu is not active yet
00238       if (imu_active_)
00239         ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
00240       else if (my_filter_.isInitialized())
00241         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());
00242       else 
00243         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());
00244       return;
00245     }
00246     StampedTransform base_imu_offset;
00247     robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset);
00248     imu_meas_ = imu_meas_ * base_imu_offset;
00249 
00250     imu_time_  = Time::now();
00251 
00252     // manually set covariance untile imu sends covariance
00253     if (imu_covariance_(1,1) == 0.0){
00254       SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
00255       measNoiseImu_Cov(1,1) = pow(0.00017,2);  // = 0.01 degrees / sec
00256       measNoiseImu_Cov(2,2) = pow(0.00017,2);  // = 0.01 degrees / sec
00257       measNoiseImu_Cov(3,3) = pow(0.00017,2);  // = 0.01 degrees / sec
00258       imu_covariance_ = measNoiseImu_Cov;
00259     }
00260 
00261     my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, base_footprint_frame_, "imu"), imu_covariance_);
00262     
00263     // activate imu
00264     if (!imu_active_) {
00265       if (!imu_initializing_){
00266         imu_initializing_ = true;
00267         imu_init_stamp_ = imu_stamp_;
00268         ROS_INFO("Initializing Imu sensor");      
00269       }
00270       if ( filter_stamp_ >= imu_init_stamp_){
00271         imu_active_ = true;
00272         imu_initializing_ = false;
00273         ROS_INFO("Imu sensor activated");      
00274       }
00275       else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.", 
00276                     (imu_init_stamp_ - filter_stamp_).toSec());
00277     }
00278     
00279     if (debug_){
00280       // write to file
00281       double tmp, yaw;
00282       imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp); 
00283       imu_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< yaw << endl;
00284     }
00285   };
00286 
00287 
00288 
00289 
00290   // callback function for VO data
00291   void OdomEstimationNode::voCallback(const VoConstPtr& vo)
00292   {
00293     vo_callback_counter_++;
00294 
00295     assert(vo_used_);
00296 
00297     // get data
00298     vo_stamp_ = vo->header.stamp;
00299     vo_time_  = Time::now();
00300     poseMsgToTF(vo->pose.pose, vo_meas_);
00301     for (unsigned int i=0; i<6; i++)
00302       for (unsigned int j=0; j<6; j++)
00303         vo_covariance_(i+1, j+1) = vo->pose.covariance[6*i+j];
00304     my_filter_.addMeasurement(StampedTransform(vo_meas_.inverse(), vo_stamp_, base_footprint_frame_, "vo"), vo_covariance_);
00305     
00306     // activate vo
00307     if (!vo_active_) {
00308       if (!vo_initializing_){
00309         vo_initializing_ = true;
00310         vo_init_stamp_ = vo_stamp_;
00311         ROS_INFO("Initializing Vo sensor");      
00312       }
00313       if (filter_stamp_ >= vo_init_stamp_){
00314         vo_active_ = true;
00315         vo_initializing_ = false;
00316         ROS_INFO("Vo sensor activated");      
00317       }
00318       else ROS_DEBUG("Waiting to activate VO, because VO measurements are still %f sec in the future.", 
00319                     (vo_init_stamp_ - filter_stamp_).toSec());
00320     }
00321     
00322     if (debug_){
00323       // write to file
00324       double Rx, Ry, Rz;
00325       vo_meas_.getBasis().getEulerYPR(Rz, Ry, Rx);
00326       vo_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< vo_meas_.getOrigin().x() << " " << vo_meas_.getOrigin().y() << " " << vo_meas_.getOrigin().z() << " "
00327                << Rx << " " << Ry << " " << Rz << endl;
00328     }
00329   };
00330 
00331 
00332   void OdomEstimationNode::gpsCallback(const GpsConstPtr& gps)
00333   {
00334     gps_callback_counter_++;
00335 
00336     assert(gps_used_);
00337 
00338     // get data
00339     gps_stamp_ = gps->header.stamp;
00340     gps_time_  = Time::now();
00341     poseMsgToTF(gps->pose.pose, gps_meas_);
00342     for (unsigned int i=0; i<3; i++)
00343       for (unsigned int j=0; j<3; j++)
00344         gps_covariance_(i+1, j+1) = gps->pose.covariance[6*i+j];
00345     my_filter_.addMeasurement(StampedTransform(gps_meas_.inverse(), gps_stamp_, base_footprint_frame_, "gps"), gps_covariance_);
00346     
00347     // activate gps
00348     if (!gps_active_) {
00349       if (!gps_initializing_){
00350             gps_initializing_ = true;
00351             gps_init_stamp_ = gps_stamp_;
00352             ROS_INFO("Initializing GPS sensor");      
00353       }
00354       if (filter_stamp_ >= gps_init_stamp_){
00355             gps_active_ = true;
00356             gps_initializing_ = false;
00357             ROS_INFO("GPS sensor activated");      
00358       }
00359       else ROS_DEBUG("Waiting to activate GPS, because GPS measurements are still %f sec in the future.", 
00360                     (gps_init_stamp_ - filter_stamp_).toSec());
00361     }
00362     
00363     if (debug_){
00364       // write to file
00365       gps_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< gps_meas_.getOrigin().x() << " " << gps_meas_.getOrigin().y() << " " << gps_meas_.getOrigin().z() <<endl;
00366     }
00367   };
00368 
00369 
00370 
00371   // filter loop
00372   void OdomEstimationNode::spin(const ros::TimerEvent& e)
00373   {
00374     ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec());
00375 
00376     // check for timing problems
00377     if ( (odom_initializing_ || odom_active_) && (imu_initializing_ || imu_active_) ){
00378       double diff = fabs( Duration(odom_stamp_ - imu_stamp_).toSec() );
00379       if (diff > 1.0) ROS_ERROR("Timestamps of odometry and imu are %f seconds apart.", diff);
00380     }
00381     
00382     // initial value for filter stamp; keep this stamp when no sensors are active
00383     filter_stamp_ = Time::now();
00384     
00385     // check which sensors are still active
00386     if ((odom_active_ || odom_initializing_) && 
00387         (Time::now() - odom_time_).toSec() > timeout_){
00388       odom_active_ = false; odom_initializing_ = false;
00389       ROS_INFO("Odom sensor not active any more");
00390     }
00391     if ((imu_active_ || imu_initializing_) && 
00392         (Time::now() - imu_time_).toSec() > timeout_){
00393       imu_active_ = false;  imu_initializing_ = false;
00394       ROS_INFO("Imu sensor not active any more");
00395     }
00396     if ((vo_active_ || vo_initializing_) && 
00397         (Time::now() - vo_time_).toSec() > timeout_){
00398       vo_active_ = false;  vo_initializing_ = false;
00399       ROS_INFO("VO sensor not active any more");
00400     }
00401 
00402     if ((gps_active_ || gps_initializing_) && 
00403         (Time::now() - gps_time_).toSec() > timeout_){
00404       gps_active_ = false;  gps_initializing_ = false;
00405       ROS_INFO("GPS sensor not active any more");
00406     }
00407 
00408     
00409     // only update filter when one of the sensors is active
00410     if (odom_active_ || imu_active_ || vo_active_ || gps_active_){
00411       
00412       // update filter at time where all sensor measurements are available
00413       if (odom_active_)  filter_stamp_ = min(filter_stamp_, odom_stamp_);
00414       if (imu_active_)   filter_stamp_ = min(filter_stamp_, imu_stamp_);
00415       if (vo_active_)    filter_stamp_ = min(filter_stamp_, vo_stamp_);
00416       if (gps_active_)  filter_stamp_ = min(filter_stamp_, gps_stamp_);
00417 
00418       
00419       // update filter
00420       if ( my_filter_.isInitialized() )  {
00421         bool diagnostics = true;
00422         if (my_filter_.update(odom_active_, imu_active_,gps_active_, vo_active_,  filter_stamp_, diagnostics)){
00423           
00424           // output most recent estimate and relative covariance
00425           my_filter_.getEstimate(output_);
00426           pose_pub_.publish(output_);
00427           ekf_sent_counter_++;
00428           
00429           // broadcast most recent estimate to TransformArray
00430           StampedTransform tmp;
00431           my_filter_.getEstimate(ros::Time(), tmp);
00432           if(!vo_active_ && !gps_active_)
00433             tmp.getOrigin().setZ(0.0);
00434           odom_broadcaster_.sendTransform(StampedTransform(tmp, tmp.stamp_, output_frame_, base_footprint_frame_));
00435           
00436           if (debug_){
00437             // write to file
00438             ColumnVector estimate; 
00439             my_filter_.getEstimate(estimate);
00440             corr_file_ << fixed << setprecision(5)<<ros::Time::now().toSec()<<" ";
00441             
00442             for (unsigned int i=1; i<=6; i++)
00443             corr_file_ << estimate(i) << " ";
00444             corr_file_ << endl;
00445           }
00446         }
00447         if (self_diagnose_ && !diagnostics)
00448           ROS_WARN("Robot pose ekf diagnostics discovered a potential problem");
00449       }
00450 
00451 
00452       // initialize filer with odometry frame
00453       if (imu_active_ && gps_active_ && !my_filter_.isInitialized()) {
00454         Quaternion q = imu_meas_.getRotation();
00455         Vector3 p = gps_meas_.getOrigin();
00456         Transform init_meas_ = Transform(q, p);
00457         my_filter_.initialize(init_meas_, gps_stamp_);
00458         ROS_INFO("Kalman filter initialized with gps and imu measurement");
00459       } 
00460       else if ( odom_active_  && !gps_used_ && !my_filter_.isInitialized()){
00461         my_filter_.initialize(odom_meas_, odom_stamp_);
00462         ROS_INFO("Kalman filter initialized with odom measurement");
00463       }
00464       else if ( vo_active_ && !gps_used_ && !my_filter_.isInitialized()){
00465         my_filter_.initialize(vo_meas_, vo_stamp_);
00466         ROS_INFO("Kalman filter initialized with vo measurement");
00467       }
00468     }
00469   };
00470 
00471 
00472 bool OdomEstimationNode::getStatus(robot_pose_ekf::GetStatus::Request& req, robot_pose_ekf::GetStatus::Response& resp)
00473 {
00474   stringstream ss;
00475   ss << "Input:" << endl;
00476   ss << " * Odometry sensor" << endl;
00477   ss << "   - is "; if (!odom_active_) ss << "NOT "; ss << "active" << endl;
00478   ss << "   - received " << odom_callback_counter_ << " messages" << endl;
00479   ss << "   - listens to topic " << odom_sub_.getTopic() << endl;
00480   ss << " * IMU sensor" << endl;
00481   ss << "   - is "; if (!imu_active_) ss << "NOT "; ss << "active" << endl;
00482   ss << "   - received " << imu_callback_counter_ << " messages" << endl;
00483   ss << "   - listens to topic " << imu_sub_.getTopic() << endl;
00484   ss << " * Visual Odometry sensor" << endl;
00485   ss << "   - is "; if (!vo_active_) ss << "NOT "; ss << "active" << endl;
00486   ss << "   - received " << vo_callback_counter_ << " messages" << endl;
00487   ss << "   - listens to topic " << vo_sub_.getTopic() << endl;
00488   ss << "Output:" << endl;
00489   ss << " * Robot pose ekf filter" << endl;
00490   ss << "   - is "; if (!my_filter_.isInitialized()) ss << "NOT "; ss << "active" << endl;
00491   ss << "   - sent " << ekf_sent_counter_ << " messages" << endl;
00492   ss << "   - pulishes on topics " << pose_pub_.getTopic() << " and /tf" << endl;
00493   resp.status = ss.str();
00494   return true;
00495 }
00496 
00497 }; // namespace
00498 
00499 
00500 
00501 
00502 
00503 
00504 // ----------
00505 // -- MAIN --
00506 // ----------
00507 using namespace estimation;
00508 int main(int argc, char **argv)
00509 {
00510   // Initialize ROS
00511   ros::init(argc, argv, "robot_pose_ekf");
00512 
00513   // create filter class
00514   OdomEstimationNode my_filter_node;
00515 
00516   ros::spin();
00517   
00518   return 0;
00519 }


robot_pose_ekf
Author(s): Wim Meeussen, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:38