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("sensor_timeout", timeout_, 1.0);
00079     nh_private.param("odom_used", odom_used_, true);
00080     nh_private.param("imu_used",  imu_used_, true);
00081     nh_private.param("vo_used",   vo_used_, true);
00082     nh_private.param("gps_used",   gps_used_, false);
00083     nh_private.param("debug",   debug_, false);
00084     nh_private.param("self_diagnose",  self_diagnose_, false);
00085     double freq;
00086     nh_private.param("freq", freq, 30.0);
00087 
00088     tf_prefix_ = tf::getPrefixParam(nh_private);
00089     output_frame_ = tf::resolve(tf_prefix_, output_frame_);
00090     base_footprint_frame_ = tf::resolve(tf_prefix_, "base_footprint");
00091 
00092     timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this);
00093 
00094     // advertise our estimation
00095     pose_pub_ = nh_private.advertise<geometry_msgs::PoseWithCovarianceStamped>(output_frame_, 10);
00096 
00097     // initialize
00098     filter_stamp_ = Time::now();
00099 
00100     // subscribe to odom messages
00101     if (odom_used_){
00102       ROS_DEBUG("Odom sensor can be used");
00103       odom_sub_ = nh.subscribe("odom", 10, &OdomEstimationNode::odomCallback, this);
00104     }
00105     else ROS_DEBUG("Odom sensor will NOT be used");
00106 
00107     // subscribe to imu messages
00108     if (imu_used_){
00109       ROS_DEBUG("Imu sensor can be used");
00110       imu_sub_ = nh.subscribe("imu_data", 10,  &OdomEstimationNode::imuCallback, this);
00111     }
00112     else ROS_DEBUG("Imu sensor will NOT be used");
00113 
00114     // subscribe to vo messages
00115     if (vo_used_){
00116       ROS_DEBUG("VO sensor can be used");
00117       vo_sub_ = nh.subscribe("vo", 10, &OdomEstimationNode::voCallback, this);
00118     }
00119     else ROS_DEBUG("VO sensor will NOT be used");
00120 
00121     if (gps_used_){
00122       ROS_DEBUG("GPS sensor can be used");
00123       gps_sub_ = nh.subscribe("gps", 10, &OdomEstimationNode::gpsCallback, this);
00124     }
00125     else ROS_DEBUG("GPS sensor will NOT be used");
00126 
00127 
00128     // publish state service
00129     state_srv_ = nh_private.advertiseService("get_status", &OdomEstimationNode::getStatus, this);
00130 
00131     if (debug_){
00132       // open files for debugging
00133       odom_file_.open("/tmp/odom_file.txt");
00134       imu_file_.open("/tmp/imu_file.txt");
00135       vo_file_.open("/tmp/vo_file.txt");
00136       gps_file_.open("/tmp/gps_file.txt");
00137       corr_file_.open("/tmp/corr_file.txt");
00138 
00139   
00140     }
00141   };
00142 
00143 
00144 
00145 
00146   // destructor
00147   OdomEstimationNode::~OdomEstimationNode(){
00148 
00149     if (debug_){
00150       // close files for debugging
00151       odom_file_.close();
00152       imu_file_.close();
00153       gps_file_.close();
00154       vo_file_.close();
00155       corr_file_.close();
00156     }
00157   };
00158 
00159 
00160 
00161 
00162 
00163   // callback function for odom data
00164   void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)
00165   {
00166     odom_callback_counter_++;
00167 
00168     ROS_DEBUG("Odom callback at time %f ", ros::Time::now().toSec());
00169     assert(odom_used_);
00170 
00171     // receive data 
00172     odom_stamp_ = odom->header.stamp;
00173     odom_time_  = Time::now();
00174     Quaternion q;
00175     tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
00176     odom_meas_  = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
00177     for (unsigned int i=0; i<6; i++)
00178       for (unsigned int j=0; j<6; j++)
00179         odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];
00180 
00181     my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, "base_footprint", "wheelodom"), odom_covariance_);
00182     
00183     // activate odom
00184     if (!odom_active_) {
00185       if (!odom_initializing_){
00186         odom_initializing_ = true;
00187         odom_init_stamp_ = odom_stamp_;
00188         ROS_INFO("Initializing Odom sensor");      
00189       }
00190       if ( filter_stamp_ >= odom_init_stamp_){
00191         odom_active_ = true;
00192         odom_initializing_ = false;
00193         ROS_INFO("Odom sensor activated");      
00194       }
00195       else ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.", 
00196                     (odom_init_stamp_ - filter_stamp_).toSec());
00197     }
00198     
00199     if (debug_){
00200       // write to file
00201       double tmp, yaw;
00202       odom_meas_.getBasis().getEulerYPR(yaw, tmp, tmp);
00203       odom_file_<< fixed <<setprecision(5) << ros::Time::now().toSec() << " " << odom_meas_.getOrigin().x() << " " << odom_meas_.getOrigin().y() << "  " << yaw << "  " << endl;
00204     }
00205   };
00206 
00207 
00208 
00209 
00210   // callback function for imu data
00211   void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
00212   {
00213     imu_callback_counter_++;
00214 
00215     assert(imu_used_);
00216 
00217     // receive data 
00218     imu_stamp_ = imu->header.stamp;
00219     tf::Quaternion orientation;
00220     quaternionMsgToTF(imu->orientation, orientation);
00221     imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
00222     for (unsigned int i=0; i<3; i++)
00223       for (unsigned int j=0; j<3; j++)
00224         imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
00225 
00226     // Transforms imu data to base_footprint frame
00227     if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
00228       // warn when imu was already activated, not when imu is not active yet
00229       if (imu_active_)
00230         ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
00231       else if (my_filter_.isInitialized())
00232         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());
00233       else 
00234         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());
00235       return;
00236     }
00237     StampedTransform base_imu_offset;
00238     robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset);
00239     imu_meas_ = imu_meas_ * base_imu_offset;
00240 
00241     imu_time_  = Time::now();
00242 
00243     // manually set covariance untile imu sends covariance
00244     if (imu_covariance_(1,1) == 0.0){
00245       SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;
00246       measNoiseImu_Cov(1,1) = pow(0.00017,2);  // = 0.01 degrees / sec
00247       measNoiseImu_Cov(2,2) = pow(0.00017,2);  // = 0.01 degrees / sec
00248       measNoiseImu_Cov(3,3) = pow(0.00017,2);  // = 0.01 degrees / sec
00249       imu_covariance_ = measNoiseImu_Cov;
00250     }
00251 
00252     my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, "base_footprint", "imu"), imu_covariance_);
00253     
00254     // activate imu
00255     if (!imu_active_) {
00256       if (!imu_initializing_){
00257         imu_initializing_ = true;
00258         imu_init_stamp_ = imu_stamp_;
00259         ROS_INFO("Initializing Imu sensor");      
00260       }
00261       if ( filter_stamp_ >= imu_init_stamp_){
00262         imu_active_ = true;
00263         imu_initializing_ = false;
00264         ROS_INFO("Imu sensor activated");      
00265       }
00266       else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.", 
00267                     (imu_init_stamp_ - filter_stamp_).toSec());
00268     }
00269     
00270     if (debug_){
00271       // write to file
00272       double tmp, yaw;
00273       imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp); 
00274       imu_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< yaw << endl;
00275     }
00276   };
00277 
00278 
00279 
00280 
00281   // callback function for VO data
00282   void OdomEstimationNode::voCallback(const VoConstPtr& vo)
00283   {
00284     vo_callback_counter_++;
00285 
00286     assert(vo_used_);
00287 
00288     // get data
00289     vo_stamp_ = vo->header.stamp;
00290     vo_time_  = Time::now();
00291     poseMsgToTF(vo->pose.pose, vo_meas_);
00292     for (unsigned int i=0; i<6; i++)
00293       for (unsigned int j=0; j<6; j++)
00294         vo_covariance_(i+1, j+1) = vo->pose.covariance[6*i+j];
00295     my_filter_.addMeasurement(StampedTransform(vo_meas_.inverse(), vo_stamp_, "base_footprint", "vo"), vo_covariance_);
00296     
00297     // activate vo
00298     if (!vo_active_) {
00299       if (!vo_initializing_){
00300         vo_initializing_ = true;
00301         vo_init_stamp_ = vo_stamp_;
00302         ROS_INFO("Initializing Vo sensor");      
00303       }
00304       if (filter_stamp_ >= vo_init_stamp_){
00305         vo_active_ = true;
00306         vo_initializing_ = false;
00307         ROS_INFO("Vo sensor activated");      
00308       }
00309       else ROS_DEBUG("Waiting to activate VO, because VO measurements are still %f sec in the future.", 
00310                     (vo_init_stamp_ - filter_stamp_).toSec());
00311     }
00312     
00313     if (debug_){
00314       // write to file
00315       double Rx, Ry, Rz;
00316       vo_meas_.getBasis().getEulerYPR(Rz, Ry, Rx);
00317       vo_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< vo_meas_.getOrigin().x() << " " << vo_meas_.getOrigin().y() << " " << vo_meas_.getOrigin().z() << " "
00318                << Rx << " " << Ry << " " << Rz << endl;
00319     }
00320   };
00321 
00322 
00323   void OdomEstimationNode::gpsCallback(const GpsConstPtr& gps)
00324   {
00325     gps_callback_counter_++;
00326 
00327     assert(gps_used_);
00328 
00329     // get data
00330     gps_stamp_ = gps->header.stamp;
00331     gps_time_  = Time::now();
00332     poseMsgToTF(gps->pose.pose, gps_meas_);
00333     for (unsigned int i=0; i<3; i++)
00334       for (unsigned int j=0; j<3; j++)
00335         gps_covariance_(i+1, j+1) = gps->pose.covariance[6*i+j];
00336     my_filter_.addMeasurement(StampedTransform(gps_meas_.inverse(), gps_stamp_, "base_footprint", "gps"), gps_covariance_);
00337     
00338     // activate gps
00339     if (!gps_active_) {
00340       if (!gps_initializing_){
00341             gps_initializing_ = true;
00342             gps_init_stamp_ = gps_stamp_;
00343             ROS_INFO("Initializing GPS sensor");      
00344       }
00345       if (filter_stamp_ >= gps_init_stamp_){
00346             gps_active_ = true;
00347             gps_initializing_ = false;
00348             ROS_INFO("GPS sensor activated");      
00349       }
00350       else ROS_DEBUG("Waiting to activate GPS, because GPS measurements are still %f sec in the future.", 
00351                     (gps_init_stamp_ - filter_stamp_).toSec());
00352     }
00353     
00354     if (debug_){
00355       // write to file
00356       gps_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< gps_meas_.getOrigin().x() << " " << gps_meas_.getOrigin().y() << " " << gps_meas_.getOrigin().z() <<endl;
00357     }
00358   };
00359 
00360 
00361 
00362   // filter loop
00363   void OdomEstimationNode::spin(const ros::TimerEvent& e)
00364   {
00365     ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec());
00366 
00367     // check for timing problems
00368     if ( (odom_initializing_ || odom_active_) && (imu_initializing_ || imu_active_) ){
00369       double diff = fabs( Duration(odom_stamp_ - imu_stamp_).toSec() );
00370       if (diff > 1.0) ROS_ERROR("Timestamps of odometry and imu are %f seconds apart.", diff);
00371     }
00372     
00373     // initial value for filter stamp; keep this stamp when no sensors are active
00374     filter_stamp_ = Time::now();
00375     
00376     // check which sensors are still active
00377     if ((odom_active_ || odom_initializing_) && 
00378         (Time::now() - odom_time_).toSec() > timeout_){
00379       odom_active_ = false; odom_initializing_ = false;
00380       ROS_INFO("Odom sensor not active any more");
00381     }
00382     if ((imu_active_ || imu_initializing_) && 
00383         (Time::now() - imu_time_).toSec() > timeout_){
00384       imu_active_ = false;  imu_initializing_ = false;
00385       ROS_INFO("Imu sensor not active any more");
00386     }
00387     if ((vo_active_ || vo_initializing_) && 
00388         (Time::now() - vo_time_).toSec() > timeout_){
00389       vo_active_ = false;  vo_initializing_ = false;
00390       ROS_INFO("VO sensor not active any more");
00391     }
00392 
00393     if ((gps_active_ || gps_initializing_) && 
00394         (Time::now() - gps_time_).toSec() > timeout_){
00395       gps_active_ = false;  gps_initializing_ = false;
00396       ROS_INFO("GPS sensor not active any more");
00397     }
00398 
00399     
00400     // only update filter when one of the sensors is active
00401     if (odom_active_ || imu_active_ || vo_active_ || gps_active_){
00402       
00403       // update filter at time where all sensor measurements are available
00404       if (odom_active_)  filter_stamp_ = min(filter_stamp_, odom_stamp_);
00405       if (imu_active_)   filter_stamp_ = min(filter_stamp_, imu_stamp_);
00406       if (vo_active_)    filter_stamp_ = min(filter_stamp_, vo_stamp_);
00407       if (gps_active_)  filter_stamp_ = min(filter_stamp_, gps_stamp_);
00408 
00409       
00410       // update filter
00411       if ( my_filter_.isInitialized() )  {
00412         bool diagnostics = true;
00413         if (my_filter_.update(odom_active_, imu_active_,gps_active_, vo_active_,  filter_stamp_, diagnostics)){
00414           
00415           // output most recent estimate and relative covariance
00416           my_filter_.getEstimate(output_);
00417           pose_pub_.publish(output_);
00418           ekf_sent_counter_++;
00419           
00420           // broadcast most recent estimate to TransformArray
00421           StampedTransform tmp;
00422           my_filter_.getEstimate(ros::Time(), tmp);
00423           if(!vo_active_ && !gps_active_)
00424             tmp.getOrigin().setZ(0.0);
00425           odom_broadcaster_.sendTransform(StampedTransform(tmp, tmp.stamp_, output_frame_, base_footprint_frame_));
00426           
00427           if (debug_){
00428             // write to file
00429             ColumnVector estimate; 
00430             my_filter_.getEstimate(estimate);
00431             corr_file_ << fixed << setprecision(5)<<ros::Time::now().toSec()<<" ";
00432             
00433             for (unsigned int i=1; i<=6; i++)
00434             corr_file_ << estimate(i) << " ";
00435             corr_file_ << endl;
00436           }
00437         }
00438         if (self_diagnose_ && !diagnostics)
00439           ROS_WARN("Robot pose ekf diagnostics discovered a potential problem");
00440       }
00441 
00442 
00443       // initialize filer with odometry frame
00444       if (imu_active_ && gps_active_ && !my_filter_.isInitialized()) {
00445         Quaternion q = imu_meas_.getRotation();
00446         Vector3 p = gps_meas_.getOrigin();
00447         Transform init_meas_ = Transform(q, p);
00448         my_filter_.initialize(init_meas_, gps_stamp_);
00449         ROS_INFO("Kalman filter initialized with gps and imu measurement");
00450       } 
00451       else if ( odom_active_  && !gps_used_ && !my_filter_.isInitialized()){
00452         my_filter_.initialize(odom_meas_, odom_stamp_);
00453         ROS_INFO("Kalman filter initialized with odom measurement");
00454       }
00455       else if ( vo_active_ && !gps_used_ && !my_filter_.isInitialized()){
00456         my_filter_.initialize(vo_meas_, vo_stamp_);
00457         ROS_INFO("Kalman filter initialized with vo measurement");
00458       }
00459     }
00460   };
00461 
00462 
00463 bool OdomEstimationNode::getStatus(robot_pose_ekf::GetStatus::Request& req, robot_pose_ekf::GetStatus::Response& resp)
00464 {
00465   stringstream ss;
00466   ss << "Input:" << endl;
00467   ss << " * Odometry sensor" << endl;
00468   ss << "   - is "; if (!odom_active_) ss << "NOT "; ss << "active" << endl;
00469   ss << "   - received " << odom_callback_counter_ << " messages" << endl;
00470   ss << "   - listens to topic " << odom_sub_.getTopic() << endl;
00471   ss << " * IMU sensor" << endl;
00472   ss << "   - is "; if (!imu_active_) ss << "NOT "; ss << "active" << endl;
00473   ss << "   - received " << imu_callback_counter_ << " messages" << endl;
00474   ss << "   - listens to topic " << imu_sub_.getTopic() << endl;
00475   ss << " * Visual Odometry sensor" << endl;
00476   ss << "   - is "; if (!vo_active_) ss << "NOT "; ss << "active" << endl;
00477   ss << "   - received " << vo_callback_counter_ << " messages" << endl;
00478   ss << "   - listens to topic " << vo_sub_.getTopic() << endl;
00479   ss << "Output:" << endl;
00480   ss << " * Robot pose ekf filter" << endl;
00481   ss << "   - is "; if (!my_filter_.isInitialized()) ss << "NOT "; ss << "active" << endl;
00482   ss << "   - sent " << ekf_sent_counter_ << " messages" << endl;
00483   ss << "   - pulishes on topics " << pose_pub_.getTopic() << " and /tf" << endl;
00484   resp.status = ss.str();
00485   return true;
00486 }
00487 
00488 }; // namespace
00489 
00490 
00491 
00492 
00493 
00494 
00495 // ----------
00496 // -- MAIN --
00497 // ----------
00498 using namespace estimation;
00499 int main(int argc, char **argv)
00500 {
00501   // Initialize ROS
00502   ros::init(argc, argv, "robot_pose_ekf");
00503 
00504   // create filter class
00505   OdomEstimationNode my_filter_node;
00506 
00507   ros::spin();
00508   
00509   return 0;
00510 }


robot_pose_ekf
Author(s): Wim Meeussen
autogenerated on Mon Oct 6 2014 02:48:09