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 #include "OdometryROS.h"
00029
00030 #include <sensor_msgs/Image.h>
00031 #include <sensor_msgs/image_encodings.h>
00032 #include <sensor_msgs/PointCloud2.h>
00033 #include <nav_msgs/Odometry.h>
00034
00035 #include <pcl_conversions/pcl_conversions.h>
00036
00037 #include <cv_bridge/cv_bridge.h>
00038
00039 #include <rtabmap/core/Odometry.h>
00040 #include <rtabmap/core/util3d.h>
00041 #include <rtabmap/core/Memory.h>
00042 #include <rtabmap/core/Signature.h>
00043 #include "rtabmap_ros/MsgConversion.h"
00044 #include "rtabmap_ros/OdomInfo.h"
00045 #include "rtabmap/utilite/UConversion.h"
00046 #include "rtabmap/utilite/ULogger.h"
00047 #include "rtabmap/utilite/UStl.h"
00048
00049 using namespace rtabmap;
00050
00051 namespace rtabmap_ros {
00052
00053 OdometryROS::OdometryROS(int argc, char * argv[]) :
00054 odometry_(0),
00055 frameId_("base_link"),
00056 odomFrameId_("odom"),
00057 groundTruthFrameId_(""),
00058 publishTf_(true),
00059 waitForTransform_(false),
00060 paused_(false)
00061 {
00062 this->processArguments(argc, argv);
00063
00064 ros::NodeHandle nh;
00065
00066 odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
00067 odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
00068 odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
00069 odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
00070
00071 ros::NodeHandle pnh("~");
00072
00073 Transform initialPose = Transform::getIdentity();
00074 std::string initialPoseStr;
00075 pnh.param("frame_id", frameId_, frameId_);
00076 pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
00077 pnh.param("publish_tf", publishTf_, publishTf_);
00078 pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
00079 pnh.param("initial_pose", initialPoseStr, initialPoseStr);
00080 pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
00081 if(initialPoseStr.size())
00082 {
00083 std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
00084 if(values.size() == 6)
00085 {
00086 initialPose = Transform(
00087 uStr2Float(values[0]), uStr2Float(values[1]), uStr2Float(values[2]),
00088 uStr2Float(values[3]), uStr2Float(values[4]), uStr2Float(values[5]));
00089 }
00090 else
00091 {
00092 ROS_ERROR("Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
00093 "Identity will be used...", initialPoseStr.c_str());
00094 }
00095 }
00096
00097
00098
00099 rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
00100 for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00101 {
00102 std::string group = uSplit(iter->first, '/').front();
00103 if(uStrContains(group, "Odom") ||
00104 group.compare("Stereo") ||
00105 group.compare("SURF") == 0 ||
00106 group.compare("SIFT") == 0 ||
00107 group.compare("ORB") == 0 ||
00108 group.compare("FAST") == 0 ||
00109 group.compare("FREAK") == 0 ||
00110 group.compare("BRIEF") == 0 ||
00111 group.compare("GFTT") == 0 ||
00112 group.compare("BRISK") == 0)
00113 {
00114 parameters_.insert(*iter);
00115 }
00116 }
00117
00118 for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
00119 {
00120 std::string vStr;
00121 bool vBool;
00122 int vInt;
00123 double vDouble;
00124 if(pnh.getParam(iter->first, vStr))
00125 {
00126 ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
00127 iter->second = vStr;
00128 }
00129 else if(pnh.getParam(iter->first, vBool))
00130 {
00131 ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
00132 iter->second = uBool2Str(vBool);
00133 }
00134 else if(pnh.getParam(iter->first, vDouble))
00135 {
00136 ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
00137 iter->second = uNumber2Str(vDouble);
00138 }
00139 else if(pnh.getParam(iter->first, vInt))
00140 {
00141 ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
00142 iter->second = uNumber2Str(vInt);
00143 }
00144
00145 if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
00146 {
00147 ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
00148 iter->second = uNumber2Str(8);
00149 }
00150 }
00151
00152
00153 std::list<std::string> oldParameterNames;
00154 oldParameterNames.push_back("Odom/Type");
00155 oldParameterNames.push_back("Odom/MaxWords");
00156 oldParameterNames.push_back("Odom/WordsRatio");
00157 oldParameterNames.push_back("Odom/LocalHistory");
00158 oldParameterNames.push_back("Odom/NearestNeighbor");
00159 oldParameterNames.push_back("Odom/NNDR");
00160 for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
00161 {
00162 std::string vStr;
00163 if(pnh.getParam(*iter, vStr))
00164 {
00165 if(iter->compare("Odom/Type") == 0)
00166 {
00167 ROS_WARN("Parameter name changed: Odom/Type -> %s. Please update your launch file accordingly.",
00168 Parameters::kOdomFeatureType().c_str());
00169 parameters_.at(Parameters::kOdomFeatureType())= vStr;
00170 }
00171 else if(iter->compare("Odom/MaxWords") == 0)
00172 {
00173 ROS_WARN("Parameter name changed: Odom/MaxWords -> %s. Please update your launch file accordingly.",
00174 Parameters::kOdomMaxFeatures().c_str());
00175 parameters_.at(Parameters::kOdomMaxFeatures())= vStr;
00176 }
00177 else if(iter->compare("Odom/LocalHistory") == 0)
00178 {
00179 ROS_WARN("Parameter name changed: Odom/LocalHistory -> %s. Please update your launch file accordingly.",
00180 Parameters::kOdomBowLocalHistorySize().c_str());
00181 parameters_.at(Parameters::kOdomBowLocalHistorySize())= vStr;
00182 }
00183 else if(iter->compare("Odom/NearestNeighbor") == 0)
00184 {
00185 ROS_WARN("Parameter name changed: Odom/NearestNeighbor -> %s. Please update your launch file accordingly.",
00186 Parameters::kOdomBowNNType().c_str());
00187 parameters_.at(Parameters::kOdomBowNNType())= vStr;
00188 }
00189 else if(iter->compare("Odom/NNDR") == 0)
00190 {
00191 ROS_WARN("Parameter name changed: Odom/NNDR -> %s. Please update your launch file accordingly.",
00192 Parameters::kOdomBowNNDR().c_str());
00193 parameters_.at(Parameters::kOdomBowNNDR())= vStr;
00194 }
00195 }
00196 }
00197
00198 int odomStrategy = 0;
00199 Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy);
00200 if(odomStrategy == 1)
00201 {
00202 ROS_INFO("Using OdometryOpticalFlow");
00203 odometry_ = new rtabmap::OdometryOpticalFlow(parameters_);
00204 }
00205 else
00206 {
00207 ROS_INFO("Using OdometryBOW");
00208 odometry_ = new rtabmap::OdometryBOW(parameters_);
00209 }
00210 if(!initialPose.isIdentity())
00211 {
00212 odometry_->reset(initialPose);
00213 }
00214
00215 resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this);
00216 resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this);
00217 pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
00218 resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
00219 }
00220
00221 OdometryROS::~OdometryROS()
00222 {
00223 ros::NodeHandle pnh("~");
00224 for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
00225 {
00226 pnh.deleteParam(iter->first);
00227 }
00228
00229 delete odometry_;
00230 }
00231
00232 void OdometryROS::processArguments(int argc, char * argv[])
00233 {
00234 for(int i=1;i<argc;++i)
00235 {
00236 if(strcmp(argv[i], "--params") == 0)
00237 {
00238 rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
00239 rtabmap::ParametersMap parametersOdom;
00240 if(strcmp(argv[i], "--params") == 0)
00241 {
00242
00243 for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00244 {
00245 if(iter->first.find("Odom") == 0 ||
00246 uSplit(iter->first, '/').front().compare("Stereo") == 0 ||
00247 uSplit(iter->first, '/').front().compare("SURF") == 0 ||
00248 uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
00249 uSplit(iter->first, '/').front().compare("ORB") == 0 ||
00250 uSplit(iter->first, '/').front().compare("FAST") == 0 ||
00251 uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
00252 uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
00253 uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
00254 uSplit(iter->first, '/').front().compare("BRISK") == 0)
00255 {
00256 parametersOdom.insert(*iter);
00257 }
00258 }
00259 }
00260
00261 for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
00262 {
00263 std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
00264 std::cout <<
00265 str <<
00266 std::setw(60 - str.size()) <<
00267 " [" <<
00268 rtabmap::Parameters::getDescription(iter->first).c_str() <<
00269 "]" <<
00270 std::endl;
00271 }
00272 ROS_WARN("Node will now exit after showing default odometry parameters because "
00273 "argument \"--params\" is detected!");
00274 exit(0);
00275 }
00276 }
00277 }
00278
00279 void OdometryROS::processData(const SensorData & data, const std_msgs::Header & header)
00280 {
00281 if(odometry_->getPose().isNull() &&
00282 !groundTruthFrameId_.empty())
00283 {
00284 tf::StampedTransform initialPose;
00285 try
00286 {
00287 if(this->waitForTransform())
00288 {
00289 if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, header.stamp, ros::Duration(1)))
00290 {
00291 ROS_WARN("Could not get transform from %s to %s after 1 second!", groundTruthFrameId_.c_str(), frameId_.c_str());
00292 return;
00293 }
00294 }
00295 this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, header.stamp, initialPose);
00296 }
00297 catch(tf::TransformException & ex)
00298 {
00299 ROS_WARN("%s",ex.what());
00300 return;
00301 }
00302 rtabmap::Transform pose = rtabmap_ros::transformFromTF(initialPose);
00303 ROS_INFO("Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
00304 pose.prettyPrint().c_str(),
00305 groundTruthFrameId_.c_str(),
00306 frameId_.c_str());
00307 odometry_->reset(pose);
00308 }
00309
00310
00311 ros::WallTime time = ros::WallTime::now();
00312 rtabmap::OdometryInfo info;
00313 rtabmap::Transform pose = odometry_->process(data, &info);
00314 if(!pose.isNull())
00315 {
00316
00317
00318
00319 tf::Transform poseTF;
00320 rtabmap_ros::transformToTF(pose, poseTF);
00321
00322 if(publishTf_)
00323 {
00324 tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, header.stamp, odomFrameId_, frameId_));
00325 }
00326
00327 if(odomPub_.getNumSubscribers())
00328 {
00329
00330 nav_msgs::Odometry odom;
00331 odom.header.stamp = header.stamp;
00332 odom.header.frame_id = odomFrameId_;
00333 odom.child_frame_id = frameId_;
00334
00335
00336 odom.pose.pose.position.x = poseTF.getOrigin().x();
00337 odom.pose.pose.position.y = poseTF.getOrigin().y();
00338 odom.pose.pose.position.z = poseTF.getOrigin().z();
00339 tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
00340
00341
00342 odom.pose.covariance.at(0) = info.variance;
00343 odom.pose.covariance.at(7) = info.variance;
00344 odom.pose.covariance.at(14) = info.variance;
00345 odom.pose.covariance.at(21) = info.variance;
00346 odom.pose.covariance.at(28) = info.variance;
00347 odom.pose.covariance.at(35) = info.variance;
00348
00349
00350 odomPub_.publish(odom);
00351 }
00352
00353 if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryBOW*>(odometry_))
00354 {
00355 const std::multimap<int, pcl::PointXYZ> & map = ((OdometryBOW*)odometry_)->getLocalMap();
00356 pcl::PointCloud<pcl::PointXYZ> cloud;
00357 for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
00358 {
00359 cloud.push_back(iter->second);
00360 }
00361 sensor_msgs::PointCloud2 cloudMsg;
00362 pcl::toROSMsg(cloud, cloudMsg);
00363 cloudMsg.header.stamp = header.stamp;
00364 cloudMsg.header.frame_id = odomFrameId_;
00365 odomLocalMap_.publish(cloudMsg);
00366 }
00367
00368 if(odomLastFrame_.getNumSubscribers())
00369 {
00370 if(dynamic_cast<OdometryBOW*>(odometry_))
00371 {
00372 const rtabmap::Signature * s = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature();
00373 if(s)
00374 {
00375 const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
00376 pcl::PointCloud<pcl::PointXYZ> cloud;
00377 rtabmap::Transform t = data.localTransform();
00378 for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
00379 {
00380
00381 pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
00382 cloud.push_back(pt);
00383 }
00384
00385 sensor_msgs::PointCloud2 cloudMsg;
00386 pcl::toROSMsg(cloud, cloudMsg);
00387 cloudMsg.header.stamp = header.stamp;
00388 cloudMsg.header.frame_id = odomFrameId_;
00389 odomLastFrame_.publish(cloudMsg);
00390 }
00391 }
00392 else
00393 {
00394
00395 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D();
00396 if(cloud->size())
00397 {
00398 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
00399 cloudTransformed = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
00400 sensor_msgs::PointCloud2 cloudMsg;
00401 pcl::toROSMsg(*cloudTransformed, cloudMsg);
00402 cloudMsg.header.stamp = header.stamp;
00403 cloudMsg.header.frame_id = odomFrameId_;
00404 odomLastFrame_.publish(cloudMsg);
00405 }
00406 }
00407 }
00408 }
00409 else
00410 {
00411
00412
00413
00414 nav_msgs::Odometry odom;
00415 odom.header.stamp = header.stamp;
00416 odom.header.frame_id = odomFrameId_;
00417 odom.child_frame_id = frameId_;
00418
00419
00420 odomPub_.publish(odom);
00421 }
00422
00423 if(odomInfoPub_.getNumSubscribers())
00424 {
00425 rtabmap_ros::OdomInfo infoMsg;
00426 odomInfoToROS(info, infoMsg);
00427 infoMsg.header.stamp = header.stamp;
00428 infoMsg.header.frame_id = odomFrameId_;
00429 odomInfoPub_.publish(infoMsg);
00430 }
00431
00432 ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
00433 }
00434
00435 bool OdometryROS::isOdometryBOW() const
00436 {
00437 return dynamic_cast<OdometryBOW*>(odometry_) != 0;
00438 }
00439
00440 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00441 {
00442 ROS_INFO("visual_odometry: reset odom!");
00443 odometry_->reset();
00444 return true;
00445 }
00446
00447 bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros::ResetPose::Response&)
00448 {
00449 Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
00450 ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
00451 odometry_->reset(pose);
00452 return true;
00453 }
00454
00455 bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00456 {
00457 if(paused_)
00458 {
00459 ROS_WARN("visual_odometry: Already paused!");
00460 }
00461 else
00462 {
00463 paused_ = true;
00464 ROS_INFO("visual_odometry: paused!");
00465 }
00466 return true;
00467 }
00468
00469 bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00470 {
00471 if(!paused_)
00472 {
00473 ROS_WARN("visual_odometry: Already running!");
00474 }
00475 else
00476 {
00477 paused_ = false;
00478 ROS_INFO("visual_odometry: resumed!");
00479 }
00480 return true;
00481 }
00482
00483 }