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
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00050
00051 namespace estimation
00052 {
00053
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
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
00095 pose_pub_ = nh_private.advertise<geometry_msgs::PoseWithCovarianceStamped>(output_frame_, 10);
00096
00097
00098 filter_stamp_ = Time::now();
00099
00100
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
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
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
00129 state_srv_ = nh_private.advertiseService("get_status", &OdomEstimationNode::getStatus, this);
00130
00131 if (debug_){
00132
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
00147 OdomEstimationNode::~OdomEstimationNode(){
00148
00149 if (debug_){
00150
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
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
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
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
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
00211 void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
00212 {
00213 imu_callback_counter_++;
00214
00215 assert(imu_used_);
00216
00217
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
00227 if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
00228
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
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);
00247 measNoiseImu_Cov(2,2) = pow(0.00017,2);
00248 measNoiseImu_Cov(3,3) = pow(0.00017,2);
00249 imu_covariance_ = measNoiseImu_Cov;
00250 }
00251
00252 my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, "base_footprint", "imu"), imu_covariance_);
00253
00254
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
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
00282 void OdomEstimationNode::voCallback(const VoConstPtr& vo)
00283 {
00284 vo_callback_counter_++;
00285
00286 assert(vo_used_);
00287
00288
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
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
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
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
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
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
00363 void OdomEstimationNode::spin(const ros::TimerEvent& e)
00364 {
00365 ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec());
00366
00367
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
00374 filter_stamp_ = Time::now();
00375
00376
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
00401 if (odom_active_ || imu_active_ || vo_active_ || gps_active_){
00402
00403
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
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
00416 my_filter_.getEstimate(output_);
00417 pose_pub_.publish(output_);
00418 ekf_sent_counter_++;
00419
00420
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
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
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 };
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498 using namespace estimation;
00499 int main(int argc, char **argv)
00500 {
00501
00502 ros::init(argc, argv, "robot_pose_ekf");
00503
00504
00505 OdomEstimationNode my_filter_node;
00506
00507 ros::spin();
00508
00509 return 0;
00510 }