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 #ifndef RobotLocalization_RosFilter_h
00034 #define RobotLocalization_RosFilter_h
00035
00036 #include <robot_localization/filter_common.h>
00037 #include <robot_localization/filter_base.h>
00038
00039 #include <ros/ros.h>
00040 #include <std_msgs/String.h>
00041 #include <nav_msgs/Odometry.h>
00042 #include <sensor_msgs/Imu.h>
00043 #include <geometry_msgs/TwistWithCovarianceStamped.h>
00044 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00045 #include <tf/transform_listener.h>
00046 #include <tf/transform_broadcaster.h>
00047 #include <tf/message_filter.h>
00048 #include <message_filters/subscriber.h>
00049
00050 #include <XmlRpcException.h>
00051
00052 #include <Eigen/Dense>
00053
00054 #include <fstream>
00055
00056
00057 typedef boost::shared_ptr< message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped> > poseMFSubPtr;
00058 typedef boost::shared_ptr< message_filters::Subscriber<geometry_msgs::TwistWithCovarianceStamped> > twistMFSubPtr;
00059 typedef boost::shared_ptr< tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped> > poseMFPtr;
00060 typedef boost::shared_ptr< tf::MessageFilter<geometry_msgs::TwistWithCovarianceStamped> > twistMFPtr;
00061
00062
00063 std::ostream& operator<<(std::ostream& os, const tf::Vector3 &vec)
00064 {
00065 os << "(" << std::setprecision(20) << vec.getX() << " " <<
00066 vec.getY() << " " << vec.getZ() << ")\n";
00067
00068 return os;
00069 }
00070
00071 std::ostream& operator<<(std::ostream& os, const tf::Transform &trans)
00072 {
00073 tf::Matrix3x3 orientation(trans.getRotation());
00074
00075 double roll, pitch, yaw;
00076 orientation.getRPY(roll, pitch, yaw);
00077
00078 os << "Origin: " << trans.getOrigin() <<
00079 "Rotation (RPY): (" << std::setprecision(20) << roll << ", " << pitch << ", " << yaw << ")\n";
00080
00081 return os;
00082 }
00083
00084 namespace RobotLocalization
00085 {
00086 template<class Filter> class RosFilter
00087 {
00088 public:
00089
00095 RosFilter() :
00096 nhLocal_("~")
00097 {
00098
00099
00100 (void) static_cast<FilterBase*>((Filter*) 0);
00101 }
00102
00103 ~RosFilter()
00104 {
00105 poseMessageFilters_.clear();
00106 twistMessageFilters_.clear();
00107 poseTopicSubs_.clear();
00108 twistTopicSubs_.clear();
00109 }
00110
00115 bool getFilteredOdometryMessage(nav_msgs::Odometry &message)
00116 {
00117
00118 if (filter_.getInitializedStatus())
00119 {
00120
00121 const Eigen::VectorXd state = filter_.getState();
00122 const Eigen::MatrixXd estimateErrorCovariance = filter_.getEstimateErrorCovariance();
00123
00124
00125
00126 tf::Quaternion quat;
00127 quat.setRPY(state(StateMemberRoll), state(StateMemberPitch), state(StateMemberYaw));
00128
00129
00130 message.pose.pose.position.x = state(StateMemberX);
00131 message.pose.pose.position.y = state(StateMemberY);
00132 message.pose.pose.position.z = state(StateMemberZ);
00133 message.pose.pose.orientation.x = quat.x();
00134 message.pose.pose.orientation.y = quat.y();
00135 message.pose.pose.orientation.z = quat.z();
00136 message.pose.pose.orientation.w = quat.w();
00137 message.twist.twist.linear.x = state(StateMemberVx);
00138 message.twist.twist.linear.y = state(StateMemberVy);
00139 message.twist.twist.linear.z = state(StateMemberVz);
00140 message.twist.twist.angular.x = state(StateMemberVroll);
00141 message.twist.twist.angular.y = state(StateMemberVpitch);
00142 message.twist.twist.angular.z = state(StateMemberVyaw);
00143
00144
00145 for (size_t i = 0; i < POSE_SIZE; i++)
00146 {
00147 for (size_t j = 0; j < POSE_SIZE; j++)
00148 {
00149 message.pose.covariance[POSE_SIZE * i + j] = estimateErrorCovariance(i, j);
00150 }
00151 }
00152
00153
00154
00155
00156 for (size_t i = 0; i < TWIST_SIZE; i++)
00157 {
00158 for (size_t j = 0; j < TWIST_SIZE; j++)
00159 {
00160 message.twist.covariance[TWIST_SIZE * i + j] = estimateErrorCovariance(i + POSITION_V_OFFSET, j + POSITION_V_OFFSET);
00161 }
00162 }
00163
00164 message.header.stamp = ros::Time::now();
00165 message.header.frame_id = worldFrameId_;
00166 message.child_frame_id = baseLinkFrameId_;
00167 }
00168
00169 return filter_.getInitializedStatus();
00170 }
00171
00174 void loadParams()
00175 {
00176
00177 bool debug;
00178 nhLocal_.param("debug", debug, false);
00179
00180 if (debug)
00181 {
00182 std::string debugOutFile;
00183 nhLocal_.param("debug_out_file", debugOutFile, std::string("robot_localization_debug.txt"));
00184 debugStream_.open(debugOutFile.c_str());
00185
00186
00187 if(debugStream_.is_open())
00188 {
00189 filter_.setDebug(debug, &debugStream_);
00190 }
00191 else
00192 {
00193 ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file " << debugOutFile);
00194 }
00195 }
00196
00197
00198 nh_.param("/tf_prefix", tfPrefix_, std::string(""));
00199
00200 if (tfPrefix_.empty())
00201 {
00202 nh_.param("tf_prefix", tfPrefix_, std::string(""));
00203 }
00204
00205
00206
00207 nhLocal_.param("map_frame", mapFrameId_, std::string("map"));
00208 nhLocal_.param("odom_frame", odomFrameId_, std::string("odom"));
00209 nhLocal_.param("base_link_frame", baseLinkFrameId_, std::string("base_link"));
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236 nhLocal_.param("world_frame", worldFrameId_, odomFrameId_);
00237
00238 ROS_FATAL_COND(mapFrameId_ == odomFrameId_ ||
00239 odomFrameId_ == baseLinkFrameId_ ||
00240 mapFrameId_ == baseLinkFrameId_,
00241 "Invalid frame configuration! The values for map_frame, odom_frame, and base_link_frame must be unique");
00242
00243
00244 if (!tfPrefix_.empty() && baseLinkFrameId_.at(0) != '/')
00245 {
00246 if(mapFrameId_.at(0) != '/')
00247 {
00248 mapFrameId_ = "/" + tfPrefix_ + "/" + mapFrameId_;
00249 }
00250
00251 if(odomFrameId_.at(0) != '/')
00252 {
00253 odomFrameId_ = "/" + tfPrefix_ + "/" + odomFrameId_;
00254 }
00255
00256 if(baseLinkFrameId_.at(0) != '/')
00257 {
00258 baseLinkFrameId_ = "/" + tfPrefix_ + "/" + baseLinkFrameId_;
00259 }
00260 }
00261
00262
00263 double sensorTimeout;
00264 nhLocal_.param("frequency", frequency_, 30.0);
00265 nhLocal_.param("sensor_timeout", sensorTimeout, 1.0 / frequency_);
00266 filter_.setSensorTimeout(sensorTimeout);
00267
00268
00269 if (filter_.getDebug())
00270 {
00271 debugStream_ << "tf_prefix is " << tfPrefix_ << "\n" << "odom_frame is " << odomFrameId_ << "\n" << "base_link_frame is " << baseLinkFrameId_
00272 << "\n" << "frequency is " << frequency_ << "\n" << "sensor_timeout is " << filter_.getSensorTimeout() << "\n";
00273 }
00274
00275
00276 setPoseSub_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("set_pose", 1, &RosFilter<Filter>::setPoseCallback, this);
00277
00278
00279 filter_.setLastMeasurementTime(ros::Time::now().toSec());
00280 filter_.setLastUpdateTime(ros::Time::now().toSec());
00281
00282
00283
00284 size_t topicInd = 0;
00285 bool moreParams = false;
00286 do
00287 {
00288
00289
00290
00291 std::stringstream ss;
00292 ss << "odom" << topicInd++;
00293 std::string odomTopicName = ss.str();
00294 moreParams = nhLocal_.hasParam(odomTopicName);
00295
00296 if (moreParams)
00297 {
00298
00299 bool differential;
00300 nhLocal_.param(odomTopicName + std::string("_differential"), differential, false);
00301
00303 if(!differential)
00304 {
00305 XmlRpc::XmlRpcValue diffConfig;
00306
00307 if (nhLocal_.hasParam(odomTopicName + std::string("_differential")))
00308 {
00309 try
00310 {
00311 nhLocal_.getParam(odomTopicName + std::string("_differential"), diffConfig);
00312
00313 if(diffConfig.getType() == XmlRpc::XmlRpcValue::TypeArray)
00314 {
00315 ROS_WARN_STREAM("Per-variable configuration of differential settings is deprecated. " <<
00316 "Please set the value for " << odomTopicName << std::string("_differential") <<
00317 " to \"true\" or \"false.\"");
00318
00319
00320 for (int i = 0; i < diffConfig.size() && !differential; i++)
00321 {
00322
00323
00324
00325 differential = differential || static_cast<int>(static_cast<bool>(diffConfig[i]));
00326 }
00327
00328 if(differential)
00329 {
00330 ROS_WARN_STREAM("Setting " << odomTopicName << std::string("_differential") << " to true");
00331 }
00332 }
00333 }
00334 catch(...)
00335 {
00336
00337 }
00338 }
00339 }
00341
00342
00343
00344 std::string odomTopic;
00345 nhLocal_.getParam(odomTopicName, odomTopic);
00346
00347
00348
00349
00350 std::vector<int> updateVec = loadUpdateConfig(odomTopicName);
00351 std::vector<int> poseUpdateVec = updateVec;
00352 std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET, poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0);
00353 std::vector<int> twistUpdateVec = updateVec;
00354 std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0);
00355
00356
00357
00358
00359
00360
00361
00362 odomTopicSubs_.push_back(
00363 nh_.subscribe<nav_msgs::Odometry>(odomTopic, 1,
00364 boost::bind(&RosFilter<Filter>::odometryCallback, this, _1, odomTopicName, updateVec, differential)));
00365
00366 poseMFPtr poseFilPtr(new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(tfListener_, worldFrameId_, 1));
00367 poseFilPtr->registerCallback(boost::bind(&RosFilter<Filter>::poseCallback, this, _1, odomTopicName, worldFrameId_, poseUpdateVec, differential));
00368 poseMessageFilters_[odomTopicName + "_pose"] = poseFilPtr;
00369
00370 twistMFPtr twistFilPtr(new tf::MessageFilter<geometry_msgs::TwistWithCovarianceStamped>(tfListener_, baseLinkFrameId_, 1));
00371 twistFilPtr->registerCallback(boost::bind(&RosFilter<Filter>::twistCallback, this, _1, odomTopicName, baseLinkFrameId_, twistUpdateVec));
00372 twistMessageFilters_[odomTopicName + "_twist"] = twistFilPtr;
00373
00374 if(filter_.getDebug())
00375 {
00376 debugStream_ << "Subscribed to " << odomTopic << "\n";
00377 }
00378 }
00379 } while (moreParams);
00380
00381
00382 topicInd = 0;
00383 moreParams = false;
00384 do
00385 {
00386 std::stringstream ss;
00387 ss << "pose" << topicInd++;
00388 std::string poseTopicName = ss.str();
00389 moreParams = nhLocal_.hasParam(poseTopicName);
00390
00391 if (moreParams)
00392 {
00393 bool differential;
00394 nhLocal_.param(poseTopicName + std::string("_differential"), differential, false);
00395
00397 if(!differential)
00398 {
00399 XmlRpc::XmlRpcValue diffConfig;
00400
00401 if (nhLocal_.hasParam(poseTopicName + std::string("_differential")))
00402 {
00403 try
00404 {
00405 nhLocal_.getParam(poseTopicName + std::string("_differential"), diffConfig);
00406
00407 if(diffConfig.getType() == XmlRpc::XmlRpcValue::TypeArray)
00408 {
00409 ROS_WARN_STREAM("Per-variable configuration of differential settings is deprecated. " <<
00410 "Please set the value for " << poseTopicName << std::string("_differential") <<
00411 " to \"true\" or \"false.\"");
00412
00413
00414 for (int i = 0; i < diffConfig.size() && !differential; i++)
00415 {
00416
00417
00418
00419 differential = differential || static_cast<int>(static_cast<bool>(diffConfig[i]));
00420 }
00421
00422 if(differential)
00423 {
00424 ROS_WARN_STREAM("Setting " << poseTopicName << std::string("_differential") << " to true");
00425 }
00426 }
00427 }
00428 catch(...)
00429 {
00430
00431 }
00432 }
00433 }
00435
00436 std::string poseTopic;
00437 nhLocal_.getParam(poseTopicName, poseTopic);
00438
00439
00440 std::vector<int> updateVec = loadUpdateConfig(poseTopicName);
00441 std::vector<int> poseUpdateVec = updateVec;
00442 std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET, poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0);
00443
00444
00445 poseMFSubPtr subPtr(new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>());
00446 subPtr->subscribe(nh_, poseTopic, 1);
00447 poseMFPtr filPtr(new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*subPtr, tfListener_, worldFrameId_, 1));
00448 filPtr->registerCallback(boost::bind(&RosFilter<Filter>::poseCallback, this, _1, poseTopicName, worldFrameId_, poseUpdateVec, differential));
00449 poseTopicSubs_.push_back(subPtr);
00450 poseMessageFilters_[poseTopicName] = filPtr;
00451
00452 if (filter_.getDebug())
00453 {
00454 debugStream_ << "Subscribed to " << poseTopic << "\n";
00455 }
00456 }
00457 } while (moreParams);
00458
00459
00460 topicInd = 0;
00461 moreParams = false;
00462 do
00463 {
00464 std::stringstream ss;
00465 ss << "twist" << topicInd++;
00466 std::string twistTopicName = ss.str();
00467 moreParams = nhLocal_.hasParam(twistTopicName);
00468
00469 if (moreParams)
00470 {
00471 std::string twistTopic;
00472 nhLocal_.getParam(twistTopicName, twistTopic);
00473
00474
00475 std::vector<int> updateVec = loadUpdateConfig(twistTopicName);
00476 std::vector<int> twistUpdateVec = updateVec;
00477 std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0);
00478
00479
00480 twistMFSubPtr subPtr(new message_filters::Subscriber<geometry_msgs::TwistWithCovarianceStamped>());
00481 subPtr->subscribe(nh_, twistTopic, 1);
00482 twistMFPtr filPtr(new tf::MessageFilter<geometry_msgs::TwistWithCovarianceStamped>(*subPtr, tfListener_, baseLinkFrameId_, 1));
00483 filPtr->registerCallback(boost::bind(&RosFilter<Filter>::twistCallback, this, _1, twistTopicName, baseLinkFrameId_, twistUpdateVec));
00484 twistTopicSubs_.push_back(subPtr);
00485 twistMessageFilters_[twistTopicName] = filPtr;
00486
00487 if (filter_.getDebug())
00488 {
00489 debugStream_ << "Subscribed to " << twistTopic << "\n";
00490 }
00491 }
00492 } while (moreParams);
00493
00494
00495 topicInd = 0;
00496 moreParams = false;
00497 do
00498 {
00499 std::stringstream ss;
00500 ss << "imu" << topicInd++;
00501 std::string imuTopicName = ss.str();
00502 moreParams = nhLocal_.hasParam(imuTopicName);
00503
00504 if (moreParams)
00505 {
00506 bool differential;
00507 nhLocal_.param(imuTopicName + std::string("_differential"), differential, false);
00508
00510 if(!differential)
00511 {
00512 XmlRpc::XmlRpcValue diffConfig;
00513
00514 if (nhLocal_.hasParam(imuTopicName + std::string("_differential")))
00515 {
00516 try
00517 {
00518 nhLocal_.getParam(imuTopicName + std::string("_differential"), diffConfig);
00519
00520 if(diffConfig.getType() == XmlRpc::XmlRpcValue::TypeArray)
00521 {
00522 ROS_WARN_STREAM("Per-variable configuration of differential settings is deprecated. " <<
00523 "Please set the value for " << imuTopicName << std::string("_differential") <<
00524 " to \"true\" or \"false.\"");
00525
00526
00527 for (int i = 0; i < diffConfig.size() && !differential; i++)
00528 {
00529
00530
00531
00532 differential = differential || static_cast<int>(static_cast<bool>(diffConfig[i]));
00533 }
00534
00535 if(differential)
00536 {
00537 ROS_WARN_STREAM("Setting " << imuTopicName << std::string("_differential") << " to true");
00538 }
00539 }
00540 }
00541 catch(...)
00542 {
00543
00544 }
00545 }
00546 }
00548
00549 std::string imuTopic;
00550 nhLocal_.getParam(imuTopicName, imuTopic);
00551
00552
00553
00554 std::vector<int> updateVec = loadUpdateConfig(imuTopicName);
00555 std::vector<int> poseUpdateVec = updateVec;
00556 std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET, poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0);
00557 std::vector<int> twistUpdateVec = updateVec;
00558 std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0);
00559
00560
00561 imuTopicSubs_.push_back(
00562 nh_.subscribe<sensor_msgs::Imu>(imuTopic, 1,
00563 boost::bind(&RosFilter<Filter>::imuCallback, this, _1, imuTopicName, updateVec, differential)));
00564
00565
00566 poseMFPtr poseFilPtr(new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(tfListener_, baseLinkFrameId_, 1));
00567 poseFilPtr->registerCallback(boost::bind(&RosFilter<Filter>::poseCallback, this, _1, imuTopicName, baseLinkFrameId_, poseUpdateVec, differential));
00568 poseMessageFilters_[imuTopicName + "_pose"] = poseFilPtr;
00569
00570 twistMFPtr twistFilPtr(new tf::MessageFilter<geometry_msgs::TwistWithCovarianceStamped>(tfListener_, baseLinkFrameId_, 1));
00571 twistFilPtr->registerCallback(boost::bind(&RosFilter<Filter>::twistCallback, this, _1, imuTopicName, baseLinkFrameId_, twistUpdateVec));
00572 twistMessageFilters_[imuTopicName + "_twist"] = twistFilPtr;
00573
00574 if (filter_.getDebug())
00575 {
00576 debugStream_ << "Subscribed to " << imuTopic << "\n";
00577 }
00578 }
00579 } while (moreParams);
00580
00581
00582 Eigen::MatrixXd processNoiseCovariance(STATE_SIZE, STATE_SIZE);
00583 XmlRpc::XmlRpcValue processNoiseCovarConfig;
00584
00585 if (nhLocal_.hasParam("process_noise_covariance"))
00586 {
00587 try
00588 {
00589 nhLocal_.getParam("process_noise_covariance", processNoiseCovarConfig);
00590
00591 ROS_ASSERT(processNoiseCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
00592
00593 int matSize = processNoiseCovariance.rows();
00594
00595 if (processNoiseCovarConfig.size() != matSize * matSize)
00596 {
00597 ROS_FATAL_STREAM("ERROR: process_noise_covariance matrix must have " << matSize * matSize << " values.");
00598 }
00599
00600 for (int i = 0; i < matSize; i++)
00601 {
00602 for (int j = 0; j < matSize; j++)
00603 {
00604 processNoiseCovariance(i, j) = processNoiseCovarConfig[matSize * i + j];
00605 }
00606 }
00607
00608 if (filter_.getDebug())
00609 {
00610 debugStream_ << "Process noise covariance is:\n" << processNoiseCovariance;
00611 }
00612 }
00613 catch (XmlRpc::XmlRpcException &e)
00614 {
00615 ROS_ERROR_STREAM(
00616 "ERROR reading sensor config: " << e.getMessage() << " for process_noise_covariance (type: " << processNoiseCovarConfig.getType() << ")");
00617 }
00618
00619 filter_.setProcessNoiseCovariance(processNoiseCovariance);
00620 }
00621 }
00622
00632 void imuCallback(const sensor_msgs::Imu::ConstPtr &msg,
00633 const std::string &topicName,
00634 const std::vector<int> &updateVector,
00635 const bool differential)
00636 {
00637 if (filter_.getDebug())
00638 {
00639 debugStream_ << "------ RosFilter::imuCallback (" << topicName << ") ------\n" <<
00640 "IMU message:\n" << *msg;
00641 }
00642
00643
00644
00645
00646
00647 std::vector<int> poseUpdateVec(STATE_SIZE, false);
00648 std::copy(updateVector.begin() + ORIENTATION_OFFSET, updateVector.begin() + ORIENTATION_OFFSET + ORIENTATION_SIZE, poseUpdateVec.begin() + ORIENTATION_SIZE);
00649 geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();
00650 posPtr->header = msg->header;
00651 posPtr->pose.pose.orientation = msg->orientation;
00652
00653
00654 for (size_t i = 0; i < ORIENTATION_SIZE; i++)
00655 {
00656 for (size_t j = 0; j < ORIENTATION_SIZE; j++)
00657 {
00658 posPtr->pose.covariance[POSE_SIZE * (i + ORIENTATION_SIZE) + (j + ORIENTATION_SIZE)] = msg->orientation_covariance[ORIENTATION_SIZE * i + j];
00659 }
00660 }
00661
00662 geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
00663 std::string imuPoseTopicName = topicName + "_pose";
00664 poseMessageFilters_[imuPoseTopicName]->add(pptr);
00665
00666 std::vector<int> twistUpdateVec(STATE_SIZE, false);
00667 std::copy(updateVector.begin() + POSITION_V_OFFSET, updateVector.end(), twistUpdateVec.begin() + POSITION_V_OFFSET);
00668 geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
00669 twistPtr->header = msg->header;
00670 twistPtr->twist.twist.angular = msg->angular_velocity;
00671
00672
00673 for (size_t i = 0; i < ORIENTATION_SIZE; i++)
00674 {
00675 for (size_t j = 0; j < ORIENTATION_SIZE; j++)
00676 {
00677 twistPtr->twist.covariance[TWIST_SIZE * (i + ORIENTATION_SIZE) + (j + ORIENTATION_SIZE)] = msg->angular_velocity_covariance[ORIENTATION_SIZE * i + j];
00678 }
00679 }
00680
00681 geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
00682 std::string imuTwistTopicName = topicName + "_twist";
00683 twistMessageFilters_[imuTwistTopicName]->add(tptr);
00684
00685 if (filter_.getDebug())
00686 {
00687 debugStream_ << "\n----- /RosFilter::imuCallback (" << topicName << ") ------\n";
00688 }
00689 }
00690
00700 void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg,
00701 const std::string &topicName,
00702 const std::vector<int> &updateVector,
00703 const bool differential)
00704 {
00705 if (filter_.getDebug())
00706 {
00707 debugStream_ << "------ RosFilter::odometryCallback (" << topicName << ") ------\n" <<
00708 "Odometry message:\n" << *msg;
00709 }
00710
00711
00712 std::vector<int> poseUpdateVec(STATE_SIZE, false);
00713 std::copy(updateVector.begin(), updateVector.begin() + POSE_SIZE, poseUpdateVec.begin());
00714 geometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();
00715 posPtr->header = msg->header;
00716 posPtr->pose = msg->pose;
00717
00718 geometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);
00719 std::string odomPoseTopicName = topicName + "_pose";
00720 poseMessageFilters_[odomPoseTopicName]->add(pptr);
00721
00722
00723 std::vector<int> twistUpdateVec(STATE_SIZE, false);
00724 std::copy(updateVector.begin() + POSITION_V_OFFSET, updateVector.end(), twistUpdateVec.begin() + POSITION_V_OFFSET);
00725 geometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();
00726 twistPtr->header = msg->header;
00727 twistPtr->header.frame_id = msg->child_frame_id;
00728 twistPtr->twist = msg->twist;
00729
00730 geometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);
00731 std::string odomTwistTopicName = topicName + "_twist";
00732 twistMessageFilters_[odomTwistTopicName]->add(tptr);
00733
00734 if (filter_.getDebug())
00735 {
00736 debugStream_ << "\n----- /RosFilter::odometryCallback (" << topicName << ") ------\n";
00737 }
00738 }
00739
00747 void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
00748 const std::string &topicName,
00749 const std::string &targetFrame,
00750 const std::vector<int> &updateVector,
00751 const bool differential)
00752 {
00753 if (filter_.getDebug())
00754 {
00755 debugStream_ << "------ RosFilter::poseCallback (" << topicName << ") ------\n" <<
00756 "Pose message:\n" << *msg;
00757 }
00758
00759
00760 if (lastMessageTimes_.count(topicName) == 0)
00761 {
00762 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
00763 }
00764
00765
00766 if (msg->header.stamp >= lastMessageTimes_[topicName])
00767 {
00768 if (filter_.getDebug())
00769 {
00770 debugStream_ << "Update vector for " << topicName << " is:\n";
00771 debugStream_ << updateVector;
00772 }
00773
00774 Eigen::VectorXd measurement(STATE_SIZE);
00775 Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
00776
00777 measurement.setZero();
00778 measurementCovariance.setZero();
00779
00780
00781 if (updateVector[StateMemberX] || updateVector[StateMemberY] || updateVector[StateMemberZ] ||
00782 updateVector[StateMemberRoll] || updateVector[StateMemberPitch] || updateVector[StateMemberYaw])
00783 {
00784 std::vector<int> updateVectorCorrected = updateVector;
00785
00786
00787 if (preparePose(msg, topicName + "_pose", targetFrame, differential, updateVectorCorrected, measurement, measurementCovariance))
00788 {
00789
00790 filter_.enqueueMeasurement(topicName + "_pose", measurement, measurementCovariance, updateVectorCorrected, msg->header.stamp.toSec());
00791
00792 if (filter_.getDebug())
00793 {
00794 debugStream_ << "Enqueued new measurement for " << topicName + "_pose\n";
00795 }
00796 }
00797 else if(filter_.getDebug())
00798 {
00799 debugStream_ << "Did *not* enqueue measurement for " << topicName + "_pose\n";
00800 }
00801 }
00802 else
00803 {
00804 if (filter_.getDebug())
00805 {
00806 debugStream_ << "Update vector for " << topicName << " is such that none of its state variables will be updated\n";
00807 }
00808 }
00809
00810 lastMessageTimes_[topicName] = msg->header.stamp;
00811
00812 if (filter_.getDebug())
00813 {
00814 debugStream_ << "Last message time for " << topicName << " is now " << lastMessageTimes_[topicName] << "\n";
00815 }
00816 }
00817 else
00818 {
00819 if (filter_.getDebug())
00820 {
00821 debugStream_ << "Message is too old. Last message time for " << topicName << " is " << lastMessageTimes_[topicName] << ", current message time is "
00822 << msg->header.stamp << ".\n";
00823 }
00824 }
00825
00826 if (filter_.getDebug())
00827 {
00828 debugStream_ << "\n----- /RosFilter::poseCallback (" << topicName << ") ------\n";
00829 }
00830 }
00831
00835 void setPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
00836 {
00837 if (filter_.getDebug())
00838 {
00839 debugStream_ << "------ RosFilter::setPoseCallback ------\n";
00840 debugStream_ << "Pose message:\n";
00841 debugStream_ << *msg;
00842 }
00843
00844 std::string topicName("setPose");
00845
00846
00847 previousMeasurements_.clear();
00848
00849
00850 previousStates_.clear();
00851
00852
00853
00854 tf::Transform empty;
00855 empty.setIdentity();
00856 previousMeasurements_.insert(std::pair<std::string, tf::Transform>(topicName, empty));
00857 previousStates_.insert(std::pair<std::string, tf::Transform>(topicName, empty));
00858
00859
00860 Eigen::VectorXd measurement(STATE_SIZE);
00861 Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
00862 std::vector<int> updateVector(STATE_SIZE, true);
00863
00864
00865 measurement.setZero();
00866
00867
00868 measurementCovariance.setIdentity();
00869 measurementCovariance *= 1e-6;
00870
00871
00872 preparePose(msg, topicName, odomFrameId_, false, updateVector, measurement, measurementCovariance);
00873
00874
00875 filter_.setState(measurement);
00876 filter_.setEstimateErrorCovariance(measurementCovariance);
00877
00878 if (filter_.getDebug())
00879 {
00880 debugStream_ << "\n------ /RosFilter::setPoseCallback ------\n";
00881 }
00882 }
00883
00890 void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
00891 const std::string &topicName,
00892 const std::string &targetFrame,
00893 const std::vector<int> &updateVector)
00894 {
00895 if (filter_.getDebug())
00896 {
00897 debugStream_ << "------ RosFilter::twistCallback (" << topicName << ") ------\n";
00898 debugStream_ << "Twist message:\n";
00899 debugStream_ << *msg;
00900 }
00901
00902 if (lastMessageTimes_.count(topicName) == 0)
00903 {
00904 lastMessageTimes_.insert(std::pair<std::string, ros::Time>(topicName, msg->header.stamp));
00905 }
00906
00907
00908 if (msg->header.stamp >= lastMessageTimes_[topicName])
00909 {
00910 if (filter_.getDebug())
00911 {
00912 debugStream_ << "Update vector for " << topicName << " is:\n";
00913 debugStream_ << updateVector;
00914 }
00915
00916 Eigen::VectorXd measurement(STATE_SIZE);
00917 Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);
00918
00919 measurement.setZero();
00920 measurementCovariance.setZero();
00921
00922
00923 if (updateVector[StateMemberVx] || updateVector[StateMemberVy] || updateVector[StateMemberVz] ||
00924 updateVector[StateMemberVroll] || updateVector[StateMemberVpitch] || updateVector[StateMemberVyaw])
00925 {
00926 std::vector<int> updateVectorCorrected = updateVector;
00927
00928
00929 if (prepareTwist(msg, topicName + "_twist", targetFrame, updateVectorCorrected, measurement, measurementCovariance))
00930 {
00931
00932 filter_.enqueueMeasurement(topicName + "_twist", measurement, measurementCovariance, updateVectorCorrected, msg->header.stamp.toSec());
00933 }
00934 }
00935 else
00936 {
00937 if (filter_.getDebug())
00938 {
00939 debugStream_ << "Update vector for " << topicName << " is such that none of its state variables will be updated\n";
00940 }
00941 }
00942
00943 lastMessageTimes_[topicName] = msg->header.stamp;
00944
00945 if (filter_.getDebug())
00946 {
00947 debugStream_ << "Last message time for " << topicName << " is now " << lastMessageTimes_[topicName] << "\n";
00948 }
00949 }
00950 else
00951 {
00952 if (filter_.getDebug())
00953 {
00954 debugStream_ << "Message is too old. Last message time for " << topicName <<
00955 " is " << lastMessageTimes_[topicName] << ", current message time is " <<
00956 msg->header.stamp << ".\n";
00957 }
00958 }
00959
00960 if (filter_.getDebug())
00961 {
00962 debugStream_ << "\n----- /RosFilter::twistCallback (" << topicName << ") ------\n";
00963 }
00964 }
00965
00968 void run()
00969 {
00970 ros::Time::init();
00971
00972 loadParams();
00973
00974
00975
00976 tf::StampedTransform mapOdomTrans;
00977 tf::StampedTransform odomBaseLinkTrans;
00978 geometry_msgs::TransformStamped mapOdomTransMsg;
00979
00980
00981 tf::transformTFToMsg(tf::Transform::getIdentity(), worldBaseLinkTransMsg_.transform);
00982 tf::transformTFToMsg(tf::Transform::getIdentity(), mapOdomTransMsg.transform);
00983
00984
00985 ros::Publisher positionPub = nh_.advertise<nav_msgs::Odometry>("odometry/filtered", 20);
00986 tf::TransformBroadcaster worldTransformBroadcaster;
00987
00988 ros::Rate loop_rate(frequency_);
00989
00990 std::map<std::string, Eigen::VectorXd> postUpdateStates;
00991
00992 while (ros::ok())
00993 {
00994
00995 nav_msgs::Odometry filteredPosition;
00996
00997 if (getFilteredOdometryMessage(filteredPosition))
00998 {
00999 worldBaseLinkTransMsg_.header.stamp = filteredPosition.header.stamp;
01000 worldBaseLinkTransMsg_.header.frame_id = filteredPosition.header.frame_id;
01001 worldBaseLinkTransMsg_.child_frame_id = filteredPosition.child_frame_id;
01002
01003 worldBaseLinkTransMsg_.transform.translation.x = filteredPosition.pose.pose.position.x;
01004 worldBaseLinkTransMsg_.transform.translation.y = filteredPosition.pose.pose.position.y;
01005 worldBaseLinkTransMsg_.transform.translation.z = filteredPosition.pose.pose.position.z;
01006 worldBaseLinkTransMsg_.transform.rotation = filteredPosition.pose.pose.orientation;
01007
01008 if(filteredPosition.header.frame_id == odomFrameId_)
01009 {
01010 worldTransformBroadcaster.sendTransform(worldBaseLinkTransMsg_);
01011 }
01012 else if(filteredPosition.header.frame_id == mapFrameId_)
01013 {
01014 try
01015 {
01016 tf::StampedTransform worldBaseLinkTrans;
01017 tf::transformStampedMsgToTF(worldBaseLinkTransMsg_, worldBaseLinkTrans);
01018
01019 tfListener_.lookupTransform(baseLinkFrameId_, odomFrameId_, ros::Time(0), odomBaseLinkTrans);
01020
01021
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031
01032
01033
01034
01035 mapOdomTrans.setData(odomBaseLinkTrans.inverse() * worldBaseLinkTrans.inverse());
01036 tf::transformStampedTFToMsg(mapOdomTrans, mapOdomTransMsg);
01037 mapOdomTransMsg.header.stamp = filteredPosition.header.stamp;
01038 mapOdomTransMsg.header.frame_id = mapFrameId_;
01039 mapOdomTransMsg.child_frame_id = odomFrameId_;
01040
01041 worldTransformBroadcaster.sendTransform(mapOdomTransMsg);
01042 }
01043 catch(...)
01044 {
01045 ROS_ERROR_STREAM("Could not obtain transform from " << odomFrameId_ << "->" << baseLinkFrameId_);
01046 }
01047 }
01048 else
01049 {
01050 ROS_ERROR_STREAM("Odometry message frame_id was " << filteredPosition.header.frame_id <<
01051 ", expected " << mapFrameId_ << " or " << odomFrameId_);
01052 }
01053
01054
01055 positionPub.publish(filteredPosition);
01056 }
01057
01058
01059 ros::spinOnce();
01060
01061
01062 filter_.integrateMeasurements(ros::Time::now().toSec(),
01063 postUpdateStates);
01064
01065
01066
01067 std::map<std::string, Eigen::VectorXd>::iterator mapIt;
01068
01069 for(mapIt = postUpdateStates.begin(); mapIt != postUpdateStates.end(); ++mapIt)
01070 {
01071 tf::Transform trans;
01072 trans.setOrigin(tf::Vector3(mapIt->second(StateMemberX),
01073 mapIt->second(StateMemberY),
01074 mapIt->second(StateMemberZ)));
01075 tf::Quaternion quat;
01076 quat.setRPY(mapIt->second(StateMemberRoll),
01077 mapIt->second(StateMemberPitch),
01078 mapIt->second(StateMemberYaw));
01079 trans.setRotation(quat);
01080 previousStates_[mapIt->first] = trans;
01081 }
01082
01083 loop_rate.sleep();
01084 }
01085 }
01086
01087 protected:
01088
01089 Filter filter_;
01090
01095 std::vector<int> loadUpdateConfig(const std::string &topicName)
01096 {
01097 XmlRpc::XmlRpcValue topicConfig;
01098 std::vector<int> updateVector(STATE_SIZE, static_cast<int>(false));
01099
01100 try
01101 {
01102 nhLocal_.getParam(topicName + "_config", topicConfig);
01103
01104 ROS_ASSERT(topicConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
01105
01106 for (int i = 0; i < topicConfig.size(); i++)
01107 {
01108
01109
01110
01111 updateVector[i] = static_cast<int>(static_cast<bool>(topicConfig[i]));
01112 }
01113 }
01114 catch (XmlRpc::XmlRpcException &e)
01115 {
01116 ROS_ERROR_STREAM("ERROR reading sensor update config: " << e.getMessage() << " for topic " <<
01117 topicName << " (type: " << topicConfig.getType() << ", expected: " << XmlRpc::XmlRpcValue::TypeArray << ")");
01118 }
01119
01120 return updateVector;
01121 }
01122
01138 bool lookupTransformSafe(const std::string &targetFrame, const std::string &sourceFrame,
01139 const ros::Time &time, tf::StampedTransform &targetFrameTrans)
01140 {
01141 bool retVal = true;
01142
01143
01144 try
01145 {
01146 tfListener_.lookupTransform(targetFrame, sourceFrame, time, targetFrameTrans);
01147 }
01148 catch (tf::TransformException &ex)
01149 {
01150 if(filter_.getDebug())
01151 {
01152 debugStream_ << "WARNING: Could not obtain transform from " << sourceFrame <<
01153 " to " << targetFrame << ". Error was " << ex.what();
01154 }
01155
01156
01157
01158
01159 try
01160 {
01161 tfListener_.lookupTransform(targetFrame, sourceFrame, ros::Time(0), targetFrameTrans);
01162
01163 ROS_WARN_STREAM("Transform from " << sourceFrame << " to " << targetFrame <<
01164 " was unavailable for the time requested. Using latest instead.\n");
01165 }
01166 catch(tf::TransformException &ex)
01167 {
01168 ROS_WARN_STREAM("Could not obtain transform from " << sourceFrame <<
01169 " to " << targetFrame << ". Error was " << ex.what() << "\n");
01170
01171 retVal = false;
01172 }
01173 }
01174
01175
01176
01177
01178 if(!retVal)
01179 {
01180 std::string msgFrame = (tfPrefix_.empty() ? targetFrame : "/" + tfPrefix_ + "/" + targetFrame);
01181
01182 if(targetFrame == sourceFrame)
01183 {
01184 targetFrameTrans.setIdentity();
01185 retVal = true;
01186 }
01187 }
01188
01189 return retVal;
01190 }
01191
01202 bool preparePose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,
01203 const std::string &topicName,
01204 const std::string &targetFrame,
01205 const bool differential,
01206 std::vector<int> &updateVector,
01207 Eigen::VectorXd &measurement,
01208 Eigen::MatrixXd &measurementCovariance)
01209 {
01210 if (filter_.getDebug())
01211 {
01212 debugStream_ << "------ RosFilter::preparePose (" << topicName << ") ------\n";
01213 }
01214
01215
01216 tf::Stamped<tf::Pose> poseTmp;
01217
01218
01219 tf::Transform curMeasurement;
01220
01221
01222 poseTmp.frame_id_ = (msg->header.frame_id == "" ? targetFrame : msg->header.frame_id);
01223 poseTmp.stamp_ = msg->header.stamp;
01224
01225
01226
01227
01228
01229
01230 poseTmp.setOrigin(tf::Vector3(msg->pose.pose.position.x,
01231 msg->pose.pose.position.y,
01232 msg->pose.pose.position.z));
01233
01234 tf::Quaternion orientation;
01235
01236
01237 if(msg->pose.pose.orientation.x == 0 && msg->pose.pose.orientation.y == 0 &&
01238 msg->pose.pose.orientation.z == 0 && msg->pose.pose.orientation.w == 0)
01239 {
01240 orientation.setValue(0.0, 0.0, 0.0, 1.0);
01241
01242 if(updateVector[StateMemberRoll] || updateVector[StateMemberPitch] || updateVector[StateMemberYaw])
01243 {
01244 ROS_WARN_STREAM("The " << topicName << " message contains an invalid orientation quaternion, " <<
01245 "but its configuration is such that orientation data is being used.");
01246 }
01247 }
01248 else
01249 {
01250 tf::quaternionMsgToTF(msg->pose.pose.orientation, orientation);
01251 }
01252
01253 poseTmp.setRotation(orientation);
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263 tf::Pose maskPosePos;
01264 tf::Pose maskPoseNeg;
01265 maskPosePos.setOrigin(tf::Vector3(updateVector[StateMemberX],
01266 updateVector[StateMemberY],
01267 updateVector[StateMemberZ]));
01268
01269 maskPoseNeg.setOrigin(-maskPosePos.getOrigin());
01270
01271 tf::Quaternion maskOrientation;
01272 maskOrientation.setRPY(updateVector[StateMemberRoll],
01273 updateVector[StateMemberPitch],
01274 updateVector[StateMemberYaw]);
01275
01276 maskPosePos.setRotation(maskOrientation);
01277
01278 maskOrientation.setRPY(-updateVector[StateMemberRoll],
01279 -updateVector[StateMemberPitch],
01280 -updateVector[StateMemberYaw]);
01281
01282 maskPoseNeg.setRotation(maskOrientation);
01283
01284
01285
01286 Eigen::MatrixXd covarianceRotated(POSE_SIZE, POSE_SIZE);
01287 covarianceRotated.setZero();
01288
01289 for (size_t i = 0; i < POSE_SIZE; i++)
01290 {
01291 for (size_t j = 0; j < POSE_SIZE; j++)
01292 {
01293 covarianceRotated(i, j) = msg->pose.covariance[POSE_SIZE * i + j];
01294 }
01295 }
01296
01297 if(filter_.getDebug())
01298 {
01299 debugStream_ << "Original measurement as tf object:\n" << poseTmp <<
01300 "\nOriginal update vector:\n" << updateVector <<
01301 "\nOriginal covariance matrix:\n" << covarianceRotated << "\n";
01302 }
01303
01304
01305
01306
01307
01308
01309
01310
01311
01312
01313
01314 if(differential && previousStates_.count(topicName) == 0)
01315 {
01316 tf::Pose prevPose;
01317 tf::transformMsgToTF(worldBaseLinkTransMsg_.transform, prevPose);
01318 previousStates_.insert(std::pair<std::string, tf::Transform>(topicName, prevPose));
01319 }
01320
01321
01322 tf::StampedTransform targetFrameTrans;
01323
01324 bool canTransform = lookupTransformSafe(targetFrame, poseTmp.frame_id_, poseTmp.stamp_, targetFrameTrans);
01325
01326 if(canTransform)
01327 {
01328
01329 poseTmp.mult(targetFrameTrans, poseTmp);
01330
01331
01332 maskPosePos.setOrigin(targetFrameTrans.getBasis() * maskPosePos.getOrigin());
01333 maskPoseNeg.setOrigin(targetFrameTrans.getBasis() * maskPoseNeg.getOrigin());
01334
01335
01336 updateVector[StateMemberX] = static_cast<int>(::fabs(maskPosePos.getOrigin().getX()) >= 1e-6 || ::fabs(maskPoseNeg.getOrigin().getX()) >= 1e-6);
01337 updateVector[StateMemberY] = static_cast<int>(::fabs(maskPosePos.getOrigin().getY()) >= 1e-6 || ::fabs(maskPoseNeg.getOrigin().getY()) >= 1e-6);
01338 updateVector[StateMemberZ] = static_cast<int>(::fabs(maskPosePos.getOrigin().getZ()) >= 1e-6 || ::fabs(maskPoseNeg.getOrigin().getZ()) >= 1e-6);
01339
01340 double rollPos, pitchPos, yawPos;
01341 double rollNeg, pitchNeg, yawNeg;
01342 quatToRPY(maskPosePos.getRotation(), rollPos, pitchPos, yawPos);
01343 quatToRPY(maskPoseNeg.getRotation(), rollNeg, pitchNeg, yawNeg);
01344 updateVector[StateMemberRoll] = static_cast<int>(::fabs(rollPos) >= 1e-6 || ::fabs(rollNeg) >= 1e-6);
01345 updateVector[StateMemberPitch] = static_cast<int>(::fabs(pitchPos) >= 1e-6 || ::fabs(pitchNeg) >= 1e-6);
01346 updateVector[StateMemberYaw] = static_cast<int>(::fabs(yawPos) >= 1e-6 || ::fabs(yawNeg) >= 1e-6);
01347
01348 if(filter_.getDebug())
01349 {
01350 debugStream_ << poseTmp.frame_id_ << "->" << targetFrame << " transform:\n" << targetFrameTrans <<
01351 "\nAfter applying transform to " << targetFrame << ", update vector is:\n" << updateVector <<
01352 "\nAfter applying transform to " << targetFrame << ", measurement is:\n" << poseTmp << "\n";
01353 }
01354
01355 poseTmp.frame_id_ = targetFrame;
01356
01357
01358 curMeasurement = poseTmp;
01359
01360
01361
01362 canTransform = (!differential || previousMeasurements_.count(topicName) > 0);
01363 if(differential)
01364 {
01365 if(canTransform)
01366 {
01367
01368
01369
01370
01371
01372
01373
01374
01375 tf::Pose prevMeasurement = previousMeasurements_[topicName];
01376
01377
01378 poseTmp.setData(prevMeasurement.inverseTimes(poseTmp));
01379
01380
01381
01382
01383
01384
01385
01386
01387
01388
01389
01390
01391
01392
01393
01394
01395
01396
01397
01398
01399
01400
01401
01402
01403
01404
01405
01406
01407
01408
01409
01410
01411
01412
01413
01414
01415
01416
01417 if (filter_.getDebug())
01418 {
01419 debugStream_ << "Previous measurement:\n" << previousMeasurements_[topicName] <<
01420 "\nAfter removing previous measurement, measurement is:\n" << poseTmp << "\n";
01421 }
01422
01423
01424
01425
01426
01427
01428
01429 tf::Pose prevPose = previousStates_[topicName];
01430
01431
01432 poseTmp.setData(prevPose * poseTmp);
01433
01434 if (filter_.getDebug())
01435 {
01436 debugStream_ << "Transforming to align with state. State is:\n" << prevPose <<
01437 "\nMeasurement is now:\n" << poseTmp << "\n";
01438 }
01439 }
01440 else if(filter_.getDebug())
01441 {
01442 debugStream_ << topicName << " has no previous measurements and is being "
01443 "updated differentially. Could not transform this measurement.\n";
01444 }
01445 }
01446
01447
01448 previousMeasurements_[topicName] = curMeasurement;
01449
01450 if(canTransform)
01451 {
01452
01453
01454
01455
01456 tf::Matrix3x3 rot(targetFrameTrans.getRotation());
01457 Eigen::MatrixXd rot6d(POSE_SIZE, POSE_SIZE);
01458 rot6d.setIdentity();
01459
01460 for(size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
01461 {
01462 rot6d(rInd, 0) = rot.getRow(rInd).getX();
01463 rot6d(rInd, 1) = rot.getRow(rInd).getY();
01464 rot6d(rInd, 2) = rot.getRow(rInd).getZ();
01465 rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
01466 rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
01467 rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
01468 }
01469
01470
01471 covarianceRotated = rot6d * covarianceRotated.eval() * rot6d.transpose();
01472
01473 if (filter_.getDebug())
01474 {
01475 debugStream_ << "Transformed covariance is \n" << covarianceRotated << "\n";
01476 }
01477
01478
01479 measurement(StateMemberX) = poseTmp.getOrigin().x();
01480 measurement(StateMemberY) = poseTmp.getOrigin().y();
01481 measurement(StateMemberZ) = poseTmp.getOrigin().z();
01482
01483
01484 double roll, pitch, yaw;
01485 quatToRPY(poseTmp.getRotation(), roll, pitch, yaw);
01486 measurement(StateMemberRoll) = roll;
01487 measurement(StateMemberPitch) = pitch;
01488 measurement(StateMemberYaw) = yaw;
01489
01490 measurementCovariance.block(0, 0, POSE_SIZE, POSE_SIZE) = covarianceRotated.block(0, 0, POSE_SIZE, POSE_SIZE);
01491 }
01492 }
01493 else if(filter_.getDebug())
01494 {
01495 debugStream_ << "Could not transform measurement into " << targetFrame << ". Ignoring...";
01496 }
01497
01498 if(filter_.getDebug())
01499 {
01500 debugStream_ << "\n----- /RosFilter::preparePose (" << topicName << ") ------\n";
01501 }
01502
01503 return canTransform;
01504 }
01505
01515 bool prepareTwist(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,
01516 const std::string &topicName,
01517 const std::string &targetFrame,
01518 std::vector<int> &updateVector,
01519 Eigen::VectorXd &measurement,
01520 Eigen::MatrixXd &measurementCovariance)
01521 {
01522 if (filter_.getDebug())
01523 {
01524 debugStream_ << "------ RosFilter::prepareTwist (" << topicName << ") ------\n";
01525 }
01526
01527
01528
01529 tf::Vector3 twistLin(updateVector[StateMemberVx] ? msg->twist.twist.linear.x : 0.0,
01530 updateVector[StateMemberVy] ? msg->twist.twist.linear.y : 0.0,
01531 updateVector[StateMemberVz] ? msg->twist.twist.linear.z : 0.0);
01532 tf::Vector3 twistRot(updateVector[StateMemberVroll] ? msg->twist.twist.angular.x : 0.0,
01533 updateVector[StateMemberVpitch] ? msg->twist.twist.angular.y : 0.0,
01534 updateVector[StateMemberVyaw] ? msg->twist.twist.angular.z : 0.0);
01535
01536
01537 std::string msgFrame = (msg->header.frame_id == "" ? baseLinkFrameId_ : msg->header.frame_id);
01538
01539
01540
01541
01542
01543
01544
01545
01546
01547 tf::Vector3 maskTwistLinPos(updateVector[StateMemberVx],
01548 updateVector[StateMemberVy],
01549 updateVector[StateMemberVz]);
01550 tf::Vector3 maskTwistLinNeg(-updateVector[StateMemberVx],
01551 -updateVector[StateMemberVy],
01552 -updateVector[StateMemberVz]);
01553 tf::Vector3 maskTwistRotPos(updateVector[StateMemberVroll],
01554 updateVector[StateMemberVpitch],
01555 updateVector[StateMemberVyaw]);
01556 tf::Vector3 maskTwistRotNeg(-updateVector[StateMemberVroll],
01557 -updateVector[StateMemberVpitch],
01558 -updateVector[StateMemberVyaw]);
01559
01560
01561 Eigen::MatrixXd covarianceRotated(TWIST_SIZE, TWIST_SIZE);
01562 covarianceRotated.setZero();
01563
01564
01565 for (size_t i = 0; i < TWIST_SIZE; i++)
01566 {
01567 for (size_t j = 0; j < TWIST_SIZE; j++)
01568 {
01569 covarianceRotated(i, j) = msg->twist.covariance[TWIST_SIZE * i + j];
01570 }
01571 }
01572
01573 if(filter_.getDebug())
01574 {
01575 debugStream_ << "Original measurement as tf object:\nLinear: " << twistLin <<
01576 "\nRotational: " << twistRot <<
01577 "\nOriginal update vector:\n" << updateVector <<
01578 "\nOriginal covariance matrix:\n" << covarianceRotated << "\n";
01579 }
01580
01581
01582
01583
01584 bool canTransform = true;
01585 tf::StampedTransform targetFrameTrans;
01586
01587 lookupTransformSafe(targetFrame, msgFrame, msg->header.stamp, targetFrameTrans);
01588
01589 if(canTransform)
01590 {
01591
01592
01593 twistRot = targetFrameTrans.getBasis() * twistRot;
01594 twistLin = targetFrameTrans.getBasis() * twistLin - targetFrameTrans.getOrigin().cross(twistRot);
01595 maskTwistRotPos = targetFrameTrans.getBasis() * maskTwistRotPos;
01596 maskTwistRotNeg = targetFrameTrans.getBasis() * maskTwistRotNeg;
01597 maskTwistLinPos = targetFrameTrans.getBasis() * maskTwistLinPos;
01598 maskTwistLinNeg = targetFrameTrans.getBasis() * maskTwistLinNeg;
01599
01600
01601 updateVector[StateMemberVx] = static_cast<int>(::fabs(maskTwistLinPos.getX()) >= 1e-6 || ::fabs(maskTwistLinNeg.getX()) >= 1e-6);
01602 updateVector[StateMemberVy] = static_cast<int>(::fabs(maskTwistLinPos.getY()) >= 1e-6 || ::fabs(maskTwistLinNeg.getY()) >= 1e-6);
01603 updateVector[StateMemberVz] = static_cast<int>(::fabs(maskTwistLinPos.getZ()) >= 1e-6 || ::fabs(maskTwistLinNeg.getZ()) >= 1e-6);
01604 updateVector[StateMemberVroll] = static_cast<int>(::fabs(maskTwistRotPos.getX()) >= 1e-6 || ::fabs(maskTwistRotNeg.getX()) >= 1e-6);
01605 updateVector[StateMemberVpitch] = static_cast<int>(::fabs(maskTwistRotPos.getY()) >= 1e-6 || ::fabs(maskTwistRotNeg.getY()) >= 1e-6);
01606 updateVector[StateMemberVyaw] = static_cast<int>(::fabs(maskTwistRotPos.getZ()) >= 1e-6 || ::fabs(maskTwistRotNeg.getZ()) >= 1e-6);
01607
01608 if(filter_.getDebug())
01609 {
01610 debugStream_ << msg->header.frame_id << "->" << targetFrame << " transform:\n" << targetFrameTrans <<
01611 "\nAfter applying transform to " << targetFrame << ", update vector is:\n" << updateVector <<
01612 "\nAfter applying transform to " << targetFrame << ", measurement is:\n" <<
01613 "Linear: " << twistLin << "Rotational: " << twistRot << "\n";
01614 }
01615
01616
01617
01618
01619
01620 tf::Matrix3x3 rot(targetFrameTrans.getRotation());
01621 Eigen::MatrixXd rot6d(TWIST_SIZE, TWIST_SIZE);
01622 rot6d.setIdentity();
01623
01624 for(size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
01625 {
01626 rot6d(rInd, 0) = rot.getRow(rInd).getX();
01627 rot6d(rInd, 1) = rot.getRow(rInd).getY();
01628 rot6d(rInd, 2) = rot.getRow(rInd).getZ();
01629 rot6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
01630 rot6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
01631 rot6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
01632 }
01633
01634
01635 covarianceRotated = rot6d * covarianceRotated.eval() * rot6d.transpose();
01636
01637 if (filter_.getDebug())
01638 {
01639 debugStream_ << "Transformed covariance is \n" << covarianceRotated << "\n";
01640 }
01641
01642
01643 measurement(StateMemberVx) = twistLin.getX();
01644 measurement(StateMemberVy) = twistLin.getY();
01645 measurement(StateMemberVz) = twistLin.getZ();
01646 measurement(StateMemberVroll) = twistRot.getX();
01647 measurement(StateMemberVpitch) = twistRot.getY();
01648 measurement(StateMemberVyaw) = twistRot.getZ();
01649
01650
01651 measurementCovariance.block(POSITION_V_OFFSET, POSITION_V_OFFSET, TWIST_SIZE, TWIST_SIZE) = covarianceRotated.block(0, 0, TWIST_SIZE, TWIST_SIZE);
01652 }
01653 else if(filter_.getDebug())
01654 {
01655 debugStream_ << "Could not transform measurement into " << targetFrame << ". Ignoring...";
01656 }
01657
01658 if (filter_.getDebug())
01659 {
01660 debugStream_ << "\n----- /RosFilter::prepareTwist (" << topicName << ") ------\n";
01661 }
01662
01663 return canTransform;
01664 }
01665
01672 inline void quatToRPY(const tf::Quaternion &quat, double &roll, double &pitch, double &yaw)
01673 {
01674 tf::Matrix3x3 orTmp(quat);
01675 orTmp.getRPY(roll, pitch, yaw);
01676 }
01677
01680 double frequency_;
01681
01684 std::string tfPrefix_;
01685
01688 std::string baseLinkFrameId_;
01689
01692 std::string odomFrameId_;
01693
01696 std::string mapFrameId_;
01697
01701 std::string worldFrameId_;
01702
01711 std::map<std::string, ros::Time> lastMessageTimes_;
01712
01722 std::map<std::string, tf::Transform> previousMeasurements_;
01723
01733 std::map<std::string, tf::Transform> previousStates_;
01734
01738 std::vector<ros::Subscriber> odomTopicSubs_;
01739
01743 std::vector<poseMFSubPtr> poseTopicSubs_;
01744
01747 std::map<std::string, poseMFPtr> poseMessageFilters_;
01748
01752 std::vector<twistMFSubPtr> twistTopicSubs_;
01753
01756 std::map<std::string, twistMFPtr> twistMessageFilters_;
01757
01761 std::vector<ros::Subscriber> imuTopicSubs_;
01762
01765 ros::Subscriber setPoseSub_;
01766
01769 ros::NodeHandle nh_;
01770
01773 ros::NodeHandle nhLocal_;
01774
01781 geometry_msgs::TransformStamped worldBaseLinkTransMsg_;
01782
01785 tf::TransformListener tfListener_;
01786
01789 std::ofstream debugStream_;
01790
01791 };
01792 }
01793
01794 #endif