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 "CoreWrapper.h"
00029
00030 #include <stdio.h>
00031 #include <ros/ros.h>
00032 #include <sensor_msgs/Image.h>
00033 #include <sensor_msgs/image_encodings.h>
00034 #include <cv_bridge/cv_bridge.h>
00035 #include <nav_msgs/Path.h>
00036 #include <std_msgs/Int32MultiArray.h>
00037 #include <std_msgs/Bool.h>
00038
00039 #include <visualization_msgs/MarkerArray.h>
00040
00041 #include <rtabmap/utilite/UTimer.h>
00042 #include <rtabmap/utilite/UDirectory.h>
00043 #include <rtabmap/utilite/UFile.h>
00044 #include <rtabmap/utilite/UConversion.h>
00045 #include <rtabmap/utilite/UStl.h>
00046 #include <rtabmap/utilite/UMath.h>
00047
00048 #include <rtabmap/core/util2d.h>
00049 #include <rtabmap/core/util3d.h>
00050 #include <rtabmap/core/util3d_transforms.h>
00051 #include <rtabmap/core/util3d_surface.h>
00052 #include <rtabmap/core/Memory.h>
00053 #include <rtabmap/core/OdometryEvent.h>
00054 #include <rtabmap/core/Version.h>
00055
00056 #include <pcl_conversions/pcl_conversions.h>
00057
00058 #include <laser_geometry/laser_geometry.h>
00059
00060 #ifdef WITH_OCTOMAP_ROS
00061 #ifdef RTABMAP_OCTOMAP
00062 #include <octomap_msgs/conversions.h>
00063 #include <rtabmap/core/OctoMap.h>
00064 #endif
00065 #endif
00066
00067 #define BAD_COVARIANCE 9999
00068
00069
00070 #include "rtabmap_ros/Info.h"
00071 #include "rtabmap_ros/MapData.h"
00072 #include "rtabmap_ros/MapGraph.h"
00073 #include "rtabmap_ros/GetMap.h"
00074 #include "rtabmap_ros/PublishMap.h"
00075
00076 #include "rtabmap_ros/MsgConversion.h"
00077
00078 using namespace rtabmap;
00079
00080 CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) :
00081 paused_(false),
00082 lastPose_(Transform::getIdentity()),
00083 lastPoseIntermediate_(false),
00084 rotVariance_(0),
00085 transVariance_(0),
00086 latestNodeWasReached_(false),
00087 frameId_("base_link"),
00088 mapFrameId_("map"),
00089 odomFrameId_(""),
00090 groundTruthFrameId_(""),
00091 configPath_(""),
00092 databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
00093 waitForTransform_(true),
00094 waitForTransformDuration_(0.2),
00095 useActionForGoal_(false),
00096 genScan_(false),
00097 genScanMaxDepth_(4.0),
00098 genScanMinDepth_(0.0),
00099 scanCloudMaxPoints_(0),
00100 scanCloudNormalK_(0),
00101 flipScan_(false),
00102 mapToOdom_(rtabmap::Transform::getIdentity()),
00103 mapsManager_(true),
00104 depthSync_(0),
00105 depthExactSync_(0),
00106 depthScanSync_(0),
00107 depthScan3dSync_(0),
00108 stereoScanSync_(0),
00109 stereoScan3dSync_(0),
00110 stereoApproxSync_(0),
00111 stereoExactSync_(0),
00112 depth2Sync_(0),
00113 depthTFSync_(0),
00114 depthTFExactSync_(0),
00115 depthScanTFSync_(0),
00116 depthScan3dTFSync_(0),
00117 stereoScanTFSync_(0),
00118 stereoScan3dTFSync_(0),
00119 stereoApproxTFSync_(0),
00120 stereoExactTFSync_(0),
00121 transformThread_(0),
00122 rate_(Parameters::defaultRtabmapDetectionRate()),
00123 createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
00124 time_(ros::Time::now()),
00125 previousStamp_(0),
00126 mbClient_("move_base", true)
00127 {
00128 ros::NodeHandle nh;
00129 ros::NodeHandle pnh("~");
00130
00131 bool subscribeScan2d = false;
00132 bool subscribeScan3d = false;
00133 bool subscribeDepth = true;
00134 bool subscribeStereo = false;
00135 int depthCameras = 1;
00136 int queueSize = 10;
00137 bool publishTf = true;
00138 double tfDelay = 0.05;
00139 double tfTolerance = 0.1;
00140 std::string tfPrefix = "";
00141 bool approxSync = true;
00142
00143
00144 pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
00145 if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
00146 {
00147 ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
00148 }
00149 pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
00150 pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
00151 pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
00152 if(subscribeDepth && subscribeStereo)
00153 {
00154 ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
00155 subscribeDepth = false;
00156 }
00157 if(subscribeScan2d && subscribeScan3d)
00158 {
00159 ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
00160 subscribeScan3d = false;
00161 }
00162 if(subscribeScan2d || subscribeScan3d)
00163 {
00164 if(!subscribeDepth && !subscribeStereo)
00165 {
00166 ROS_WARN("When subscribing to laser scan, you should subscribe to depth or stereo too. Subscribing to depth by default...");
00167 subscribeDepth = true;
00168 }
00169 }
00170
00171 pnh.param("config_path", configPath_, configPath_);
00172 pnh.param("database_path", databasePath_, databasePath_);
00173
00174 pnh.param("frame_id", frameId_, frameId_);
00175 pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
00176 pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
00177 pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
00178 pnh.param("depth_cameras", depthCameras, depthCameras);
00179 pnh.param("queue_size", queueSize, queueSize);
00180 if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
00181 {
00182 ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
00183 "to \"approx_sync\"! Your value is still copied to "
00184 "corresponding parameter.");
00185 pnh.param("stereo_approx_sync", approxSync, approxSync);
00186 }
00187 else
00188 {
00189 pnh.param("approx_sync", approxSync, approxSync);
00190 }
00191
00192 pnh.param("publish_tf", publishTf, publishTf);
00193 pnh.param("tf_delay", tfDelay, tfDelay);
00194 pnh.param("tf_prefix", tfPrefix, tfPrefix);
00195 pnh.param("tf_tolerance", tfTolerance, tfTolerance);
00196 pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
00197 pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
00198 pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
00199 pnh.param("gen_scan", genScan_, genScan_);
00200 pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_);
00201 pnh.param("gen_scan_min_depth", genScanMinDepth_, genScanMinDepth_);
00202 pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
00203 pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
00204 pnh.param("flip_scan", flipScan_, flipScan_);
00205
00206 if(!tfPrefix.empty())
00207 {
00208 if(!frameId_.empty())
00209 {
00210 frameId_ = tfPrefix+"/"+frameId_;
00211 }
00212 if(!mapFrameId_.empty())
00213 {
00214 mapFrameId_ = tfPrefix+"/"+mapFrameId_;
00215 }
00216 if(!odomFrameId_.empty())
00217 {
00218 odomFrameId_ = tfPrefix+"/"+odomFrameId_;
00219 }
00220 if(!groundTruthFrameId_.empty())
00221 {
00222 groundTruthFrameId_ = tfPrefix+"/"+groundTruthFrameId_;
00223 }
00224
00225 }
00226
00227 if(depthCameras <= 0 && subscribeDepth)
00228 {
00229 depthCameras = 1;
00230 }
00231
00232 ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
00233 if(!odomFrameId_.empty())
00234 {
00235 ROS_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str());
00236 }
00237 if(!groundTruthFrameId_.empty())
00238 {
00239 ROS_INFO("rtabmap: ground_truth_frame_id = %s", groundTruthFrameId_.c_str());
00240 }
00241 ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
00242 ROS_INFO("rtabmap: queue_size = %d", queueSize);
00243 ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
00244 ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
00245 ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
00246 ROS_INFO("rtabmap: approx_sync = %s", approxSync?"true":"false");
00247
00248 infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
00249 mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
00250 mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>("mapGraph", 1);
00251 labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
00252
00253
00254 goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
00255 goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
00256 nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1);
00257 goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1);
00258 globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
00259 localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
00260
00261 ros::Publisher nextMetricGoal_;
00262 ros::Publisher goalReached_;
00263 ros::Publisher path_;
00264
00265 configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
00266 databasePath_ = uReplaceChar(databasePath_, '~', UDirectory::homeDir());
00267 if(configPath_.size() && configPath_.at(0) != '/')
00268 {
00269 configPath_ = UDirectory::currentDir(true) + configPath_;
00270 }
00271 if(databasePath_.size() && databasePath_.at(0) != '/')
00272 {
00273 databasePath_ = UDirectory::currentDir(true) + databasePath_;
00274 }
00275
00276
00277 parameters_ = loadParameters(configPath_);
00278
00279
00280 uInsert(parameters_, std::make_pair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros"));
00281 for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
00282 {
00283 std::string vStr;
00284 bool vBool;
00285 int vInt;
00286 double vDouble;
00287 if(pnh.getParam(iter->first, vStr))
00288 {
00289 ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
00290 iter->second = vStr;
00291
00292 if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
00293 {
00294 iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
00295 }
00296 else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0)
00297 {
00298 iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
00299 }
00300 }
00301 else if(pnh.getParam(iter->first, vBool))
00302 {
00303 ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
00304 iter->second = uBool2Str(vBool);
00305 }
00306 else if(pnh.getParam(iter->first, vDouble))
00307 {
00308 ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
00309 iter->second = uNumber2Str(vDouble);
00310 }
00311 else if(pnh.getParam(iter->first, vInt))
00312 {
00313 ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
00314 iter->second = uNumber2Str(vInt);
00315 }
00316 }
00317
00318
00319 for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00320 {
00321 uInsert(parameters_, ParametersPair(iter->first, iter->second));
00322 ROS_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
00323 }
00324
00325
00326 for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
00327 iter!=Parameters::getRemovedParameters().end();
00328 ++iter)
00329 {
00330 std::string vStr;
00331 if(pnh.getParam(iter->first, vStr))
00332 {
00333 if(iter->second.first)
00334 {
00335
00336 parameters_.at(iter->second.second)= vStr;
00337 ROS_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
00338 iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
00339 }
00340 else
00341 {
00342 if(iter->second.second.empty())
00343 {
00344 ROS_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore!",
00345 iter->first.c_str());
00346 }
00347 else
00348 {
00349 ROS_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
00350 iter->first.c_str(), iter->second.second.c_str());
00351 }
00352 }
00353 }
00354 }
00355
00356
00357 nh.setParam("is_rtabmap_paused", paused_);
00358 for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
00359 {
00360 nh.setParam(iter->first, iter->second);
00361 }
00362 if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end())
00363 {
00364 Parameters::parse(parameters_, Parameters::kRtabmapDetectionRate(), rate_);
00365 ROS_INFO("RTAB-Map detection rate = %f Hz", rate_);
00366 }
00367 if(parameters_.find(Parameters::kRtabmapCreateIntermediateNodes()) != parameters_.end())
00368 {
00369 Parameters::parse(parameters_, Parameters::kRtabmapCreateIntermediateNodes(), createIntermediateNodes_);
00370 if(createIntermediateNodes_)
00371 {
00372 ROS_INFO("Create intermediate nodes");
00373 }
00374 }
00375 bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
00376 if(isRGBD)
00377 {
00378
00379 if(!subscribeDepth && !subscribeStereo)
00380 {
00381 ROS_WARN("ROS param subscribe_depth and subscribe_stereo are false, but RTAB-Map "
00382 "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth or subscribe_stereo "
00383 "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure "
00384 "detection on images-only.");
00385 }
00386 }
00387
00388 if(paused_)
00389 {
00390 ROS_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
00391 }
00392
00393 if(deleteDbOnStart)
00394 {
00395 if(UFile::erase(databasePath_) == 0)
00396 {
00397 ROS_INFO("rtabmap: Deleted database \"%s\" (--delete_db_on_start is set).", databasePath_.c_str());
00398 }
00399 }
00400
00401 if(databasePath_.size())
00402 {
00403 ROS_INFO("rtabmap: Using database from \"%s\".", databasePath_.c_str());
00404 }
00405 else
00406 {
00407 ROS_INFO("rtabmap: database_path parameter not set, the map will not be saved.");
00408 }
00409
00410
00411 rtabmap_.init(parameters_, databasePath_);
00412
00413 if(databasePath_.size() && rtabmap_.getMemory())
00414 {
00415 ROS_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str());
00416 }
00417
00418
00419 updateSrv_ = nh.advertiseService("update_parameters", &CoreWrapper::updateRtabmapCallback, this);
00420 resetSrv_ = nh.advertiseService("reset", &CoreWrapper::resetRtabmapCallback, this);
00421 pauseSrv_ = nh.advertiseService("pause", &CoreWrapper::pauseRtabmapCallback, this);
00422 resumeSrv_ = nh.advertiseService("resume", &CoreWrapper::resumeRtabmapCallback, this);
00423 triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
00424 backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this);
00425 setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
00426 setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
00427 getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
00428 getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this);
00429 getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
00430 publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
00431 setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
00432 cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
00433 setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
00434 listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
00435 #ifdef WITH_OCTOMAP_ROS
00436 #ifdef RTABMAP_OCTOMAP
00437 octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
00438 octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
00439 #endif
00440 #endif
00441
00442 setLogDebugSrv_ = pnh.advertiseService("log_debug", &CoreWrapper::setLogDebug, this);
00443 setLogInfoSrv_ = pnh.advertiseService("log_info", &CoreWrapper::setLogInfo, this);
00444 setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this);
00445 setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this);
00446
00447 setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, approxSync, depthCameras);
00448
00449 int optimizeIterations = 0;
00450 Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
00451 if(publishTf && optimizeIterations != 0)
00452 {
00453 transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay, tfTolerance));
00454 }
00455 else if(publishTf)
00456 {
00457 UWARN("Graph optimization is disabled (%s=0), the tf between frame \"%s\" and odometry frame will not be published. You can safely ignore this warning if you are using map_optimizer node.",
00458 Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str());
00459 }
00460 }
00461
00462 CoreWrapper::~CoreWrapper()
00463 {
00464 if(transformThread_)
00465 {
00466 transformThread_->join();
00467 delete transformThread_;
00468 }
00469
00470 if(depthSync_)
00471 delete depthSync_;
00472 if(depthExactSync_)
00473 delete depthExactSync_;
00474 if(depthScanSync_)
00475 delete depthScanSync_;
00476 if(depthScan3dSync_)
00477 delete depthScan3dSync_;
00478 if(stereoScanSync_)
00479 delete stereoScanSync_;
00480 if(stereoScan3dSync_)
00481 delete stereoScan3dSync_;
00482 if(stereoApproxSync_)
00483 delete stereoApproxSync_;
00484 if(stereoExactSync_)
00485 delete stereoExactSync_;
00486 if(depth2Sync_)
00487 delete depth2Sync_;
00488 if(depthTFSync_)
00489 delete depthTFSync_;
00490 if(depthTFExactSync_)
00491 delete depthTFExactSync_;
00492 if(depthScanTFSync_)
00493 delete depthScanTFSync_;
00494 if(depthScan3dTFSync_)
00495 delete depthScan3dTFSync_;
00496 if(stereoScanTFSync_)
00497 delete stereoScanTFSync_;
00498 if(stereoScan3dTFSync_)
00499 delete stereoScan3dTFSync_;
00500 if(stereoApproxTFSync_)
00501 delete stereoApproxTFSync_;
00502 if(stereoExactTFSync_)
00503 delete stereoExactTFSync_;
00504
00505 for(unsigned int i=0; i<imageSubs_.size(); ++i)
00506 {
00507 delete imageSubs_[i];
00508 }
00509 imageSubs_.clear();
00510 for(unsigned int i=0; i<imageDepthSubs_.size(); ++i)
00511 {
00512 delete imageDepthSubs_[i];
00513 }
00514 imageDepthSubs_.clear();
00515 for(unsigned int i=0; i<cameraInfoSubs_.size(); ++i)
00516 {
00517 delete cameraInfoSubs_[i];
00518 }
00519 cameraInfoSubs_.clear();
00520
00521 this->saveParameters(configPath_);
00522
00523 ros::NodeHandle nh;
00524 for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
00525 {
00526 nh.deleteParam(iter->first);
00527 }
00528 nh.deleteParam("is_rtabmap_paused");
00529
00530 printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
00531 }
00532
00533 ParametersMap CoreWrapper::loadParameters(const std::string & configFile)
00534 {
00535 ParametersMap parameters = Parameters::getDefaultParameters();
00536 if(!configFile.empty())
00537 {
00538 ROS_INFO("Loading parameters from %s", configFile.c_str());
00539 if(!UFile::exists(configFile.c_str()))
00540 {
00541 ROS_WARN("Config file doesn't exist! It will be generated...");
00542 }
00543 Parameters::readINI(configFile.c_str(), parameters);
00544 }
00545
00546
00547 return parameters;
00548 }
00549
00550 void CoreWrapper::saveParameters(const std::string & configFile)
00551 {
00552 if(!configFile.empty())
00553 {
00554 printf("Saving parameters to %s\n", configFile.c_str());
00555
00556 if(!UFile::exists(configFile.c_str()))
00557 {
00558 printf("Config file doesn't exist, a new one will be created.\n");
00559 }
00560 Parameters::writeINI(configFile.c_str(), parameters_);
00561 }
00562 else
00563 {
00564 ROS_INFO("Parameters are not saved! (No configuration file provided...)");
00565 }
00566 }
00567
00568 void CoreWrapper::publishLoop(double tfDelay, double tfTolerance)
00569 {
00570 if(tfDelay == 0)
00571 return;
00572 ros::Rate r(1.0 / tfDelay);
00573 while(ros::ok())
00574 {
00575 if(!odomFrameId_.empty())
00576 {
00577 mapToOdomMutex_.lock();
00578 ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfTolerance);
00579 geometry_msgs::TransformStamped msg;
00580 msg.child_frame_id = odomFrameId_;
00581 msg.header.frame_id = mapFrameId_;
00582 msg.header.stamp = tfExpiration;
00583 rtabmap_ros::transformToGeometryMsg(mapToOdom_, msg.transform);
00584 tfBroadcaster_.sendTransform(msg);
00585 mapToOdomMutex_.unlock();
00586 }
00587 r.sleep();
00588 }
00589 }
00590
00591 void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
00592 {
00593 if(!paused_)
00594 {
00595 if(rate_>0.0f)
00596 {
00597 if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
00598 {
00599 return;
00600 }
00601 }
00602 time_ = ros::Time::now();
00603
00604 if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00605 imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00606 imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00607 imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00608 {
00609 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
00610 return;
00611 }
00612
00613 cv_bridge::CvImageConstPtr ptrImage;
00614 if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00615 imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00616 {
00617 ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
00618 }
00619 else
00620 {
00621 ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
00622 }
00623
00624
00625 UTimer timer;
00626 if(rtabmap_.isIDsGenerated() || ptrImage->header.seq > 0)
00627 {
00628 if(!rtabmap_.process(ptrImage->image.clone(), ptrImage->header.seq))
00629 {
00630 ROS_WARN("RTAB-Map could not process the data received! (ROS id = %d)", ptrImage->header.seq);
00631 }
00632 else
00633 {
00634 this->publishStats(ros::Time::now());
00635 }
00636 }
00637 else if(!rtabmap_.isIDsGenerated())
00638 {
00639 ROS_WARN("Ignoring received image because its sequence ID=0. Please "
00640 "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
00641 "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
00642 "when you need to have IDs output of RTAB-map synchronised with the source "
00643 "image sequence ID.");
00644 }
00645 ROS_INFO("rtabmap: Update rate=%fs, Limit=%fs, Processing time = %fs (%d local nodes)",
00646 1.0f/rate_,
00647 rtabmap_.getTimeThreshold()/1000.0f,
00648 timer.ticks(),
00649 rtabmap_.getWMSize()+rtabmap_.getSTMSize());
00650 }
00651 }
00652
00653 bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
00654 {
00655 if(!paused_)
00656 {
00657 Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
00658 if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || (odomMsg->pose.covariance[0] >= BAD_COVARIANCE && odomMsg->twist.covariance[0] >= BAD_COVARIANCE)))
00659 {
00660 UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0]));
00661 rtabmap_.triggerNewMap();
00662 rotVariance_ = 0;
00663 transVariance_ = 0;
00664 }
00665
00666 lastPoseIntermediate_ = false;
00667 lastPose_ = odom;
00668 lastPoseStamp_ = odomMsg->header.stamp;
00669
00670
00671 if(!odom.isNull())
00672 {
00673
00674 float transVariance = uMax3(odomMsg->twist.covariance[0], MIN(odomMsg->twist.covariance[7], BAD_COVARIANCE), MIN(odomMsg->twist.covariance[14], BAD_COVARIANCE));
00675 float rotVariance = uMax3(MIN(odomMsg->twist.covariance[21],BAD_COVARIANCE), MIN(odomMsg->twist.covariance[28], BAD_COVARIANCE), odomMsg->twist.covariance[35]);
00676 if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
00677 {
00678 rotVariance_ = rotVariance;
00679 }
00680 if(uIsFinite(transVariance) && transVariance > transVariance_)
00681 {
00682 transVariance_ = transVariance;
00683 }
00684 }
00685
00686
00687 bool ignoreFrame = false;
00688 if(rate_>0.0f)
00689 {
00690 if((previousStamp_.toSec() > 0.0 && odomMsg->header.stamp.toSec() > previousStamp_.toSec() && odomMsg->header.stamp - previousStamp_ < ros::Duration(1.0f/rate_)) ||
00691 ((previousStamp_.toSec() <= 0.0 || odomMsg->header.stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_)))
00692 {
00693 ignoreFrame = true;
00694 }
00695 }
00696 if(ignoreFrame)
00697 {
00698 if(createIntermediateNodes_)
00699 {
00700 lastPoseIntermediate_ = true;
00701 }
00702 else
00703 {
00704 return false;
00705 }
00706 }
00707 else if(!ignoreFrame)
00708 {
00709 time_ = ros::Time::now();
00710 previousStamp_ = odomMsg->header.stamp;
00711 }
00712
00713 return true;
00714 }
00715 return false;
00716 }
00717
00718 bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
00719 {
00720 if(!paused_)
00721 {
00722
00723 Transform odom = getTransform(odomFrameId_, frameId_, stamp);
00724 if(odom.isNull())
00725 {
00726 return false;
00727 }
00728
00729 if(!lastPose_.isIdentity() && odom.isIdentity())
00730 {
00731 UWARN("Odometry is reset (identity pose detected). Increment map id!");
00732 rtabmap_.triggerNewMap();
00733 rotVariance_ = 0;
00734 transVariance_ = 0;
00735 }
00736
00737 lastPoseIntermediate_ = false;
00738 lastPose_ = odom;
00739 lastPoseStamp_ = stamp;
00740
00741 bool ignoreFrame = false;
00742 if(rate_>0.0f)
00743 {
00744 if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) ||
00745 ((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_)))
00746 {
00747 ignoreFrame = true;
00748 }
00749 }
00750 if(ignoreFrame)
00751 {
00752 if(createIntermediateNodes_)
00753 {
00754 lastPoseIntermediate_ = true;
00755 }
00756 else
00757 {
00758 return false;
00759 }
00760 }
00761 else if(!ignoreFrame)
00762 {
00763 time_ = ros::Time::now();
00764 previousStamp_ = stamp;
00765 }
00766
00767 return true;
00768 }
00769 return false;
00770 }
00771
00772 Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
00773 {
00774
00775 Transform transform;
00776 try
00777 {
00778 if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_>0.0)
00779 {
00780
00781 if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
00782 {
00783 ROS_WARN("rtabmap: Could not get transform from %s to %s after %f seconds (for stamp=%f)!",
00784 fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_, stamp.toSec());
00785 return transform;
00786 }
00787 }
00788
00789 tf::StampedTransform tmp;
00790 tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
00791 transform = rtabmap_ros::transformFromTF(tmp);
00792 }
00793 catch(tf::TransformException & ex)
00794 {
00795 ROS_WARN("%s",ex.what());
00796 }
00797 return transform;
00798 }
00799
00800 void CoreWrapper::commonDepthCallback(
00801 const std::string & odomFrameId,
00802 const sensor_msgs::ImageConstPtr& imageMsg,
00803 const sensor_msgs::ImageConstPtr& depthMsg,
00804 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00805 const sensor_msgs::LaserScanConstPtr& scanMsg,
00806 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
00807 {
00808 std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
00809 std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
00810 std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
00811 imageMsgs.push_back(imageMsg);
00812 depthMsgs.push_back(depthMsg);
00813 cameraInfoMsgs.push_back(cameraInfoMsg);
00814 commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg);
00815 }
00816 void CoreWrapper::commonDepthCallback(
00817 const std::string & odomFrameId,
00818 const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
00819 const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
00820 const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
00821 const sensor_msgs::LaserScanConstPtr& scan2dMsg,
00822 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
00823 {
00824 UASSERT(imageMsgs.size()>0 &&
00825 imageMsgs.size() == depthMsgs.size() &&
00826 imageMsgs.size() == cameraInfoMsgs.size());
00827
00828
00829 Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
00830 if(odomT.isNull() && !odomFrameId_.empty())
00831 {
00832 ROS_WARN("Could not get TF transform from %s to %s, sensors will not be synchronized with odometry pose.",
00833 odomFrameId.c_str(), frameId_.c_str());
00834 }
00835
00836 int imageWidth = imageMsgs[0]->width;
00837 int imageHeight = imageMsgs[0]->height;
00838 int cameraCount = imageMsgs.size();
00839 cv::Mat rgb;
00840 cv::Mat depth;
00841 pcl::PointCloud<pcl::PointXYZ> scanCloud2d;
00842 std::vector<CameraModel> cameraModels;
00843 int genMaxScanPts = 0;
00844 for(unsigned int i=0; i<imageMsgs.size(); ++i)
00845 {
00846 if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
00847 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00848 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00849 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00850 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00851 !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
00852 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
00853 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
00854 {
00855 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
00856 return;
00857 }
00858
00859 UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
00860 uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
00861 imageWidth,
00862 imageMsgs[i]->width,
00863 imageHeight,
00864 imageMsgs[i]->height).c_str());
00865 UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
00866 uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
00867 imageWidth,
00868 depthMsgs[i]->width,
00869 imageHeight,
00870 depthMsgs[i]->height).c_str());
00871
00872 Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
00873 if(localTransform.isNull())
00874 {
00875 ROS_ERROR("TF of received depth image %d at time %fs is not set, aborting rtabmap update.", i, depthMsgs[i]->header.stamp.toSec());
00876 return;
00877 }
00878
00879 if(lastPoseStamp_ != depthMsgs[i]->header.stamp)
00880 {
00881 if(!odomT.isNull())
00882 {
00883 Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp);
00884 if(sensorT.isNull())
00885 {
00886 ROS_WARN("Could not get odometry value for depth image %d stamp (%fs). Latest odometry "
00887 "stamp is %fs. The depth image pose will not be synchronized with odometry.", i, depthMsgs[i]->header.stamp.toSec(), lastPoseStamp_.toSec());
00888 }
00889 else
00890 {
00891 localTransform = odomT.inverse() * sensorT * localTransform;
00892 }
00893 }
00894 }
00895
00896 cv_bridge::CvImageConstPtr ptrImage;
00897 if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
00898 {
00899 ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
00900 }
00901 else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00902 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00903 {
00904 ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
00905 }
00906 else
00907 {
00908 ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
00909 }
00910 cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
00911 cv::Mat subDepth = ptrDepth->image;
00912 UASSERT(uContains(parameters_, Parameters::kMemSaveDepth16Format()));
00913 if(subDepth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
00914 {
00915 subDepth = util2d::cvtDepthFromFloat(subDepth);
00916 static bool shown = false;
00917 if(!shown)
00918 {
00919 ROS_WARN("Save depth data to 16 bits format: depth type detected is "
00920 "32FC1, use 16UC1 depth format to avoid this conversion "
00921 "(or set parameter \"Mem/SaveDepth16Format=false\" to use "
00922 "32bits format). This message is only printed once...");
00923 shown = true;
00924 }
00925 }
00926
00927
00928 if(rgb.empty())
00929 {
00930 rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
00931 }
00932 if(depth.empty())
00933 {
00934 depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
00935 }
00936
00937 if(ptrImage->image.type() == rgb.type())
00938 {
00939 ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
00940 }
00941 else
00942 {
00943 ROS_ERROR("Some RGB images are not the same type!");
00944 return;
00945 }
00946
00947 if(subDepth.type() == depth.type())
00948 {
00949 subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
00950 }
00951 else
00952 {
00953 ROS_ERROR("Some Depth images are not the same type!");
00954 return;
00955 }
00956
00957 cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
00958
00959 if(scan2dMsg.get() == 0 && genScan_)
00960 {
00961 scanCloud2d += util3d::laserScanFromDepthImage(
00962 subDepth,
00963 cameraModels.back().fx(),
00964 cameraModels.back().fy(),
00965 cameraModels.back().cx(),
00966 cameraModels.back().cy(),
00967 genScanMaxDepth_,
00968 genScanMinDepth_,
00969 localTransform);
00970 genMaxScanPts += subDepth.cols;
00971 }
00972 }
00973
00974 cv::Mat scan;
00975 if(scan2dMsg.get() != 0)
00976 {
00977
00978 if(getTransform(frameId_,
00979 scan2dMsg->header.frame_id,
00980 scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull())
00981 {
00982 ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec());
00983 return;
00984 }
00985
00986
00987 sensor_msgs::PointCloud2 scanOut;
00988 laser_geometry::LaserProjection projection;
00989 projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
00990 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
00991 pcl::fromROSMsg(scanOut, *pclScan);
00992
00993
00994 if(lastPoseStamp_ != scan2dMsg->header.stamp)
00995 {
00996 if(!odomT.isNull())
00997 {
00998 Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp);
00999 if(sensorT.isNull())
01000 {
01001 ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
01002 "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
01003 }
01004 else
01005 {
01006 Transform t = odomT.inverse() * sensorT;
01007 pclScan = util3d::transformPointCloud(pclScan, t);
01008 }
01009
01010 }
01011 }
01012 scan = util3d::laserScan2dFromPointCloud(*pclScan);
01013 if(flipScan_)
01014 {
01015 cv::Mat flipScan;
01016 cv::flip(scan, flipScan, 1);
01017 scan = flipScan;
01018 }
01019 }
01020 else if(scan3dMsg.get() != 0)
01021 {
01022 bool containNormals = false;
01023 for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
01024 {
01025 if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
01026 {
01027 containNormals = true;
01028 break;
01029 }
01030 }
01031 if(containNormals)
01032 {
01033 pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
01034 pcl::fromROSMsg(*scan3dMsg, *pclScan);
01035 scan = util3d::laserScanFromPointCloud(*pclScan);
01036 }
01037 else
01038 {
01039 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
01040 pcl::fromROSMsg(*scan3dMsg, *pclScan);
01041
01042 if(scanCloudNormalK_ > 0)
01043 {
01044
01045 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
01046 pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
01047 pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
01048 scan = util3d::laserScanFromPointCloud(*pclScanNormal);
01049 }
01050 else
01051 {
01052 scan = util3d::laserScanFromPointCloud(*pclScan);
01053 }
01054 }
01055 }
01056 else if(scanCloud2d.size())
01057 {
01058 scan = util3d::laserScan2dFromPointCloud(scanCloud2d);
01059 }
01060
01061 ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp:
01062 scan3dMsg.get() != 0?scan3dMsg->header.stamp:
01063 depthMsgs[0]->header.stamp;
01064
01065 Transform groundTruthPose;
01066 if(!groundTruthFrameId_.empty())
01067 {
01068 groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp);
01069 }
01070
01071 SensorData data(scan,
01072 scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
01073 scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
01074 rgb,
01075 depth,
01076 cameraModels,
01077 lastPoseIntermediate_?-1:imageMsgs[0]->header.seq,
01078 rtabmap_ros::timestampFromROS(stamp));
01079 data.setGroundTruth(groundTruthPose);
01080
01081 process(stamp,
01082 data,
01083 lastPose_,
01084 odomFrameId,
01085 uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0,
01086 uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0);
01087 rotVariance_ = 0;
01088 transVariance_ = 0;
01089 }
01090
01091 void CoreWrapper::commonStereoCallback(
01092 const std::string & odomFrameId,
01093 const sensor_msgs::ImageConstPtr& leftImageMsg,
01094 const sensor_msgs::ImageConstPtr& rightImageMsg,
01095 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
01096 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
01097 const sensor_msgs::LaserScanConstPtr& scan2dMsg,
01098 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
01099 {
01100 if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
01101 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
01102 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
01103 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
01104 !(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
01105 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
01106 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
01107 rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
01108 {
01109 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
01110 return;
01111 }
01112
01113
01114 Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
01115 if(odomT.isNull() && !odomFrameId_.empty())
01116 {
01117 ROS_WARN("Could not get TF transform from %s to %s, sensors will not be synchronized with odometry pose.",
01118 odomFrameId.c_str(), frameId_.c_str());
01119 }
01120
01121 Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp);
01122 if(localTransform.isNull())
01123 {
01124 return;
01125 }
01126
01127
01128 if(lastPoseStamp_ != leftImageMsg->header.stamp)
01129 {
01130 if(!odomT.isNull())
01131 {
01132 Transform sensorT = getTransform(odomFrameId, frameId_, leftImageMsg->header.stamp);
01133 if(sensorT.isNull())
01134 {
01135 return;
01136 }
01137 localTransform = odomT.inverse() * sensorT * localTransform;
01138 }
01139 }
01140
01141 cv::Mat scan;
01142 if(scan2dMsg.get() != 0)
01143 {
01144
01145 if(getTransform(frameId_,
01146 scan2dMsg->header.frame_id,
01147 scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull())
01148 {
01149 ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec());
01150 return;
01151 }
01152
01153
01154 sensor_msgs::PointCloud2 scanOut;
01155 laser_geometry::LaserProjection projection;
01156
01157 projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_);
01158 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
01159 pcl::fromROSMsg(scanOut, *pclScan);
01160
01161
01162 if(lastPoseStamp_ != scan2dMsg->header.stamp)
01163 {
01164 if(!odomT.isNull())
01165 {
01166 Transform sensorT = getTransform(odomFrameId, frameId_, scan2dMsg->header.stamp);
01167 if(sensorT.isNull())
01168 {
01169 return;
01170 }
01171 Transform t = odomT.inverse() * sensorT;
01172 pclScan = util3d::transformPointCloud(pclScan, t);
01173
01174 }
01175 }
01176
01177 scan = util3d::laserScan2dFromPointCloud(*pclScan);
01178 if(flipScan_)
01179 {
01180 cv::Mat flipScan;
01181 cv::flip(scan, flipScan, 1);
01182 scan = flipScan;
01183 }
01184 }
01185 else if(scan3dMsg.get() != 0)
01186 {
01187 bool containNormals = false;
01188 for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
01189 {
01190 if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
01191 {
01192 containNormals = true;
01193 break;
01194 }
01195 }
01196 if(containNormals)
01197 {
01198 pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
01199 pcl::fromROSMsg(*scan3dMsg, *pclScan);
01200 scan = util3d::laserScanFromPointCloud(*pclScan);
01201 }
01202 else
01203 {
01204 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
01205 pcl::fromROSMsg(*scan3dMsg, *pclScan);
01206
01207 if(scanCloudNormalK_ > 0)
01208 {
01209
01210 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
01211 pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
01212 pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
01213 scan = util3d::laserScanFromPointCloud(*pclScanNormal);
01214 }
01215 else
01216 {
01217 scan = util3d::laserScanFromPointCloud(*pclScan);
01218 }
01219 }
01220 }
01221
01222 cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage;
01223 if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
01224 leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
01225 {
01226 ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8");
01227 }
01228 else
01229 {
01230 ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8");
01231 }
01232 ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8");
01233
01234 rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
01235
01236 if(stereoModel.baseline() > 10.0)
01237 {
01238 static bool shown = false;
01239 if(!shown)
01240 {
01241 ROS_WARN("Detected baseline (%f m) is quite large! Is your "
01242 "right camera_info P(0,3) correctly set? Note that "
01243 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
01244 stereoModel.baseline());
01245 shown = true;
01246 }
01247 }
01248
01249 ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp:
01250 scan3dMsg.get() != 0?scan3dMsg->header.stamp:
01251 leftImageMsg->header.stamp;
01252
01253 Transform groundTruthPose;
01254 if(!groundTruthFrameId_.empty())
01255 {
01256 groundTruthPose = getTransform(groundTruthFrameId_, frameId_, stamp);
01257 }
01258
01259 SensorData data(scan,
01260 scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
01261 scan2dMsg.get() != 0?scan2dMsg->range_max:0,
01262 ptrLeftImage->image,
01263 ptrRightImage->image,
01264 stereoModel,
01265 lastPoseIntermediate_?-1:leftImageMsg->header.seq,
01266 rtabmap_ros::timestampFromROS(stamp));
01267 data.setGroundTruth(groundTruthPose);
01268
01269 process(stamp,
01270 data,
01271 lastPose_,
01272 odomFrameId,
01273 uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0,
01274 uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0);
01275
01276 rotVariance_ = 0;
01277 transVariance_ = 0;
01278 }
01279
01280 void CoreWrapper::depthCallback(
01281 const sensor_msgs::ImageConstPtr& imageMsg,
01282 const nav_msgs::OdometryConstPtr & odomMsg,
01283 const sensor_msgs::ImageConstPtr& depthMsg,
01284 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01285 {
01286 if(!commonOdomUpdate(odomMsg))
01287 {
01288 return;
01289 }
01290
01291 sensor_msgs::LaserScanConstPtr scanMsg;
01292 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01293 commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg);
01294 }
01295 void CoreWrapper::depthScanCallback(
01296 const sensor_msgs::ImageConstPtr& imageMsg,
01297 const nav_msgs::OdometryConstPtr & odomMsg,
01298 const sensor_msgs::ImageConstPtr& depthMsg,
01299 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
01300 const sensor_msgs::LaserScanConstPtr& scanMsg)
01301 {
01302 if(!commonOdomUpdate(odomMsg))
01303 {
01304 return;
01305 }
01306 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01307 commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg);
01308 }
01309 void CoreWrapper::depthScan3dCallback(
01310 const sensor_msgs::ImageConstPtr& imageMsg,
01311 const nav_msgs::OdometryConstPtr & odomMsg,
01312 const sensor_msgs::ImageConstPtr& depthMsg,
01313 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
01314 const sensor_msgs::PointCloud2ConstPtr& scanMsg)
01315 {
01316 if(!commonOdomUpdate(odomMsg))
01317 {
01318 return;
01319 }
01320 sensor_msgs::LaserScanConstPtr scan2dMsg;
01321 commonDepthCallback(odomMsg->header.frame_id, imageMsg, depthMsg, cameraInfoMsg, scan2dMsg, scanMsg);
01322 }
01323 void CoreWrapper::stereoCallback(
01324 const sensor_msgs::ImageConstPtr& leftImageMsg,
01325 const sensor_msgs::ImageConstPtr& rightImageMsg,
01326 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
01327 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
01328 const nav_msgs::OdometryConstPtr & odomMsg)
01329 {
01330 if(!commonOdomUpdate(odomMsg))
01331 {
01332 return;
01333 }
01334
01335 sensor_msgs::LaserScanConstPtr scanMsg;
01336 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01337 commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg);
01338 }
01339 void CoreWrapper::stereoScanCallback(
01340 const sensor_msgs::ImageConstPtr& leftImageMsg,
01341 const sensor_msgs::ImageConstPtr& rightImageMsg,
01342 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
01343 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
01344 const sensor_msgs::LaserScanConstPtr& scanMsg,
01345 const nav_msgs::OdometryConstPtr & odomMsg)
01346 {
01347 if(!commonOdomUpdate(odomMsg))
01348 {
01349 return;
01350 }
01351 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01352 commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg);
01353 }
01354 void CoreWrapper::stereoScan3dCallback(
01355 const sensor_msgs::ImageConstPtr& leftImageMsg,
01356 const sensor_msgs::ImageConstPtr& rightImageMsg,
01357 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
01358 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
01359 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
01360 const nav_msgs::OdometryConstPtr & odomMsg)
01361 {
01362 if(!commonOdomUpdate(odomMsg))
01363 {
01364 return;
01365 }
01366 sensor_msgs::LaserScanConstPtr scan2dMsg;
01367 commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scanMsg);
01368 }
01369
01370 void CoreWrapper::depth2Callback(
01371 const nav_msgs::OdometryConstPtr & odomMsg,
01372 const sensor_msgs::ImageConstPtr& image1Msg,
01373 const sensor_msgs::ImageConstPtr& depth1Msg,
01374 const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
01375 const sensor_msgs::ImageConstPtr& image2Msg,
01376 const sensor_msgs::ImageConstPtr& depth2Msg,
01377 const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg)
01378 {
01379 if(!commonOdomUpdate(odomMsg))
01380 {
01381 return;
01382 }
01383
01384 std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
01385 std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
01386 std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
01387 imageMsgs.push_back(image1Msg);
01388 imageMsgs.push_back(image2Msg);
01389 depthMsgs.push_back(depth1Msg);
01390 depthMsgs.push_back(depth2Msg);
01391 cameraInfoMsgs.push_back(cameraInfo1Msg);
01392 cameraInfoMsgs.push_back(cameraInfo2Msg);
01393
01394 sensor_msgs::LaserScanConstPtr scanMsg;
01395 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01396 commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg);
01397 }
01398
01399
01400 void CoreWrapper::depthTFCallback(
01401 const sensor_msgs::ImageConstPtr& imageMsg,
01402 const sensor_msgs::ImageConstPtr& depthMsg,
01403 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
01404 {
01405 if(!commonOdomTFUpdate(depthMsg->header.stamp))
01406 {
01407 return;
01408 }
01409 sensor_msgs::LaserScanConstPtr scanMsg;
01410 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01411 commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg);
01412 }
01413 void CoreWrapper::depthScanTFCallback(
01414 const sensor_msgs::ImageConstPtr& imageMsg,
01415 const sensor_msgs::ImageConstPtr& depthMsg,
01416 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
01417 const sensor_msgs::LaserScanConstPtr& scanMsg)
01418 {
01419 if(!commonOdomTFUpdate(scanMsg->header.stamp))
01420 {
01421 return;
01422 }
01423 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01424 commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scanMsg, scan3dMsg);
01425 }
01426 void CoreWrapper::depthScan3dTFCallback(
01427 const sensor_msgs::ImageConstPtr& imageMsg,
01428 const sensor_msgs::ImageConstPtr& depthMsg,
01429 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
01430 const sensor_msgs::PointCloud2ConstPtr& scanMsg)
01431 {
01432 if(!commonOdomTFUpdate(scanMsg->header.stamp))
01433 {
01434 return;
01435 }
01436 sensor_msgs::LaserScanConstPtr scan2dMsg;
01437 commonDepthCallback(odomFrameId_, imageMsg, depthMsg, cameraInfoMsg, scan2dMsg, scanMsg);
01438 }
01439 void CoreWrapper::stereoTFCallback(
01440 const sensor_msgs::ImageConstPtr& leftImageMsg,
01441 const sensor_msgs::ImageConstPtr& rightImageMsg,
01442 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
01443 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
01444 {
01445 if(!commonOdomTFUpdate(leftImageMsg->header.stamp))
01446 {
01447 return;
01448 }
01449
01450 sensor_msgs::LaserScanConstPtr scanMsg;
01451 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01452 commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg);
01453 }
01454 void CoreWrapper::stereoScanTFCallback(
01455 const sensor_msgs::ImageConstPtr& leftImageMsg,
01456 const sensor_msgs::ImageConstPtr& rightImageMsg,
01457 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
01458 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
01459 const sensor_msgs::LaserScanConstPtr& scanMsg)
01460 {
01461 if(!commonOdomTFUpdate(leftImageMsg->header.stamp))
01462 {
01463 return;
01464 }
01465 sensor_msgs::PointCloud2ConstPtr scan3dMsg;
01466 commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg);
01467 }
01468
01469 void CoreWrapper::stereoScan3dTFCallback(
01470 const sensor_msgs::ImageConstPtr& leftImageMsg,
01471 const sensor_msgs::ImageConstPtr& rightImageMsg,
01472 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
01473 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
01474 const sensor_msgs::PointCloud2ConstPtr& scanMsg)
01475 {
01476 if(!commonOdomTFUpdate(leftImageMsg->header.stamp))
01477 {
01478 return;
01479 }
01480 sensor_msgs::LaserScanConstPtr scan2dMsg;
01481 commonStereoCallback(odomFrameId_, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scanMsg);
01482 }
01483
01484 void CoreWrapper::process(
01485 const ros::Time & stamp,
01486 const SensorData & data,
01487 const Transform & odom,
01488 const std::string & odomFrameId,
01489 float odomRotationalVariance,
01490 float odomTransitionalVariance)
01491 {
01492 UTimer timer;
01493 if(rtabmap_.isIDsGenerated() || data.id() > 0)
01494 {
01495 double timeRtabmap = 0.0;
01496 double timeUpdateMaps = 0.0;
01497 double timePublishMaps = 0.0;
01498 if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
01499 {
01500 timeRtabmap = timer.ticks();
01501 mapToOdomMutex_.lock();
01502 mapToOdom_ = rtabmap_.getMapCorrection();
01503 odomFrameId_ = odomFrameId;
01504 mapToOdomMutex_.unlock();
01505
01506 if(data.id() < 0)
01507 {
01508 ROS_INFO("Intermediate node added");
01509 }
01510 else
01511 {
01512
01513 this->publishStats(stamp);
01514 std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
01515
01516
01517 std::map<int, rtabmap::Signature> tmpSignature;
01518 SensorData tmpData = data;
01519 tmpData.setId(-1);
01520 tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
01521 filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom));
01522
01523
01524 filteredPoses = mapsManager_.updateMapCaches(
01525 filteredPoses,
01526 rtabmap_.getMemory(),
01527 false,
01528 false,
01529 false,
01530 false,
01531 false,
01532 tmpSignature);
01533
01534 timeUpdateMaps = timer.ticks();
01535
01536 mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
01537
01538
01539 if(!currentMetricGoal_.isNull())
01540 {
01541 if(rtabmap_.getPath().size() == 0)
01542 {
01543 if(rtabmap_.getPathStatus() > 0)
01544 {
01545
01546 ROS_INFO("Planning: Publishing goal reached!");
01547 }
01548 else
01549 {
01550 ROS_WARN("Planning: Plan failed!");
01551 if(mbClient_.isServerConnected())
01552 {
01553 mbClient_.cancelGoal();
01554 }
01555 }
01556 if(goalReachedPub_.getNumSubscribers())
01557 {
01558 std_msgs::Bool result;
01559 result.data = rtabmap_.getPathStatus() > 0;
01560 goalReachedPub_.publish(result);
01561 }
01562 currentMetricGoal_.setNull();
01563 latestNodeWasReached_ = false;
01564 }
01565 else
01566 {
01567 currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
01568 if(!currentMetricGoal_.isNull())
01569 {
01570
01571 if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
01572 {
01573 if(latestNodeWasReached_ ||
01574 rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
01575 {
01576 latestNodeWasReached_ = true;
01577 currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
01578 }
01579 }
01580
01581
01582 publishCurrentGoal(stamp);
01583
01584
01585 publishLocalPath(stamp);
01586
01587
01588 publishGlobalPath(stamp);
01589 }
01590 else
01591 {
01592 ROS_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)",
01593 rtabmap_.getPathCurrentGoalId());
01594 rtabmap_.clearPath(-1);
01595 if(goalReachedPub_.getNumSubscribers())
01596 {
01597 std_msgs::Bool result;
01598 result.data = false;
01599 goalReachedPub_.publish(result);
01600 }
01601 currentMetricGoal_.setNull();
01602 latestNodeWasReached_ = false;
01603 }
01604 }
01605 }
01606
01607 timePublishMaps = timer.ticks();
01608 }
01609 }
01610 else
01611 {
01612 timeRtabmap = timer.ticks();
01613 }
01614 ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
01615 rate_>0?1.0f/rate_:0,
01616 rtabmap_.getTimeThreshold()/1000.0f,
01617 timeRtabmap,
01618 timeUpdateMaps,
01619 timePublishMaps,
01620 (int)rtabmap_.getLocalOptimizedPoses().size(),
01621 rtabmap_.getWMSize()+rtabmap_.getSTMSize());
01622 }
01623 else if(!rtabmap_.isIDsGenerated())
01624 {
01625 ROS_WARN("Ignoring received image because its sequence ID=0. Please "
01626 "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
01627 "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
01628 "when you need to have IDs output of RTAB-map synchronised with the source "
01629 "image sequence ID.");
01630 }
01631 }
01632
01633 void CoreWrapper::goalCommonCallback(
01634 int id,
01635 const std::string & label,
01636 const Transform & pose,
01637 const ros::Time & stamp,
01638 double * planningTime)
01639 {
01640 UTimer timer;
01641
01642 if(id == 0 && !label.empty() && rtabmap_.getMemory())
01643 {
01644 id = rtabmap_.getMemory()->getSignatureIdByLabel(label);
01645 }
01646
01647 if(id > 0)
01648 {
01649 ROS_INFO("Planning: set goal %d", id);
01650 }
01651 else if(!pose.isNull())
01652 {
01653 ROS_INFO("Planning: set goal %s", pose.prettyPrint().c_str());
01654 }
01655
01656 if(planningTime)
01657 {
01658 *planningTime = 0.0;
01659 }
01660
01661 bool success = false;
01662 if((id > 0 && rtabmap_.computePath(id, true)) ||
01663 (!pose.isNull() && rtabmap_.computePath(pose)))
01664 {
01665 if(planningTime)
01666 {
01667 *planningTime = timer.elapsed();
01668 }
01669 ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
01670 const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
01671
01672 currentMetricGoal_.setNull();
01673 latestNodeWasReached_ = false;
01674 if(poses.size() == 0)
01675 {
01676 ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
01677 rtabmap_.getGoalReachedRadius());
01678 rtabmap_.clearPath(1);
01679 if(goalReachedPub_.getNumSubscribers())
01680 {
01681 std_msgs::Bool result;
01682 result.data = true;
01683 goalReachedPub_.publish(result);
01684 }
01685 success = true;
01686 }
01687 else
01688 {
01689 currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
01690 if(!currentMetricGoal_.isNull())
01691 {
01692 ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
01693
01694
01695 if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
01696 {
01697 if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
01698 {
01699 latestNodeWasReached_ = true;
01700 currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
01701 }
01702 }
01703
01704 publishCurrentGoal(stamp);
01705 publishLocalPath(stamp);
01706 publishGlobalPath(stamp);
01707
01708
01709 std::stringstream stream;
01710 for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
01711 {
01712 if(iter != poses.begin())
01713 {
01714 stream << " ";
01715 }
01716 stream << iter->first;
01717 }
01718 ROS_INFO("Global path: [%s]", stream.str().c_str());
01719 success=true;
01720 }
01721 else
01722 {
01723 ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
01724 }
01725 }
01726 }
01727 else if(!label.empty())
01728 {
01729 ROS_ERROR("Planning: Node with label \"%s\" not found!", label.c_str());
01730 }
01731 else if(pose.isNull())
01732 {
01733 ROS_ERROR("Planning: Node id should be > 0 !");
01734 }
01735 else
01736 {
01737 ROS_ERROR("Planning: A node near the goal's pose not found! The pose may be to far from the graph (RGBD/LocalRadius=%f m)", rtabmap_.getLocalRadius());
01738 }
01739
01740 if(!success)
01741 {
01742 rtabmap_.clearPath(-1);
01743 if(goalReachedPub_.getNumSubscribers())
01744 {
01745 std_msgs::Bool result;
01746 result.data = false;
01747 goalReachedPub_.publish(result);
01748 }
01749 }
01750 }
01751
01752 void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
01753 {
01754 Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
01755 if(targetPose.isNull())
01756 {
01757 ROS_ERROR("Pose received is null!");
01758 return;
01759 }
01760
01761
01762 if(mapFrameId_.compare(msg->header.frame_id) != 0)
01763 {
01764 Transform t = this->getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp);
01765 if(t.isNull())
01766 {
01767 ROS_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
01768 msg->header.frame_id.c_str(), mapFrameId_.c_str());
01769 return;
01770 }
01771 targetPose = t * targetPose;
01772 }
01773
01774 goalCommonCallback(0, "", targetPose, msg->header.stamp);
01775 }
01776
01777 void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
01778 {
01779 if(msg->node_id <= 0 && msg->node_label.empty())
01780 {
01781 ROS_ERROR("Node id or label should be set!");
01782 return;
01783 }
01784 goalCommonCallback(msg->node_id, msg->node_label, Transform(), msg->header.stamp);
01785 }
01786
01787 bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01788 {
01789 ros::NodeHandle nh;
01790 for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
01791 {
01792 std::string vStr;
01793 bool vBool;
01794 int vInt;
01795 double vDouble;
01796 if(nh.getParam(iter->first, vStr))
01797 {
01798 ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
01799 iter->second = vStr;
01800 }
01801 else if(nh.getParam(iter->first, vBool))
01802 {
01803 ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
01804 iter->second = uBool2Str(vBool);
01805 }
01806 else if(nh.getParam(iter->first, vInt))
01807 {
01808 ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
01809 iter->second = uNumber2Str(vInt).c_str();
01810 }
01811 else if(nh.getParam(iter->first, vDouble))
01812 {
01813 ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
01814 iter->second = uNumber2Str(vDouble).c_str();
01815 }
01816 }
01817 ROS_INFO("rtabmap: Updating parameters");
01818 if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end())
01819 {
01820 rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate()));
01821 ROS_INFO("RTAB-Map rate detection = %f Hz", rate_);
01822 }
01823 rtabmap_.parseParameters(parameters_);
01824 return true;
01825 }
01826
01827 bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01828 {
01829 ROS_INFO("rtabmap: Reset");
01830 rtabmap_.resetMemory();
01831 rotVariance_ = 0;
01832 transVariance_ = 0;
01833 lastPose_.setIdentity();
01834 lastPoseIntermediate_ = false;
01835 currentMetricGoal_.setNull();
01836 latestNodeWasReached_ = false;
01837 mapsManager_.clear();
01838 previousStamp_ = ros::Time(0);
01839 return true;
01840 }
01841
01842 bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01843 {
01844 if(paused_)
01845 {
01846 ROS_WARN("rtabmap: Already paused!");
01847 }
01848 else
01849 {
01850 paused_ = true;
01851 ROS_INFO("rtabmap: paused!");
01852 ros::NodeHandle nh;
01853 nh.setParam("is_rtabmap_paused", true);
01854 }
01855 return true;
01856 }
01857
01858 bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01859 {
01860 if(!paused_)
01861 {
01862 ROS_WARN("rtabmap: Already running!");
01863 }
01864 else
01865 {
01866 paused_ = false;
01867 ROS_INFO("rtabmap: resumed!");
01868 ros::NodeHandle nh;
01869 nh.setParam("is_rtabmap_paused", false);
01870 }
01871 return true;
01872 }
01873
01874 bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01875 {
01876 ROS_INFO("rtabmap: Trigger new map");
01877 rtabmap_.triggerNewMap();
01878 return true;
01879 }
01880
01881 bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01882 {
01883 ROS_INFO("Backup: Saving memory...");
01884 rtabmap_.close();
01885 ROS_INFO("Backup: Saving memory... done!");
01886
01887 rotVariance_ = 0;
01888 transVariance_ = 0;
01889 lastPose_.setIdentity();
01890 currentMetricGoal_.setNull();
01891 latestNodeWasReached_ = false;
01892
01893 ROS_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
01894 UFile::copy(databasePath_, databasePath_+".back");
01895 ROS_INFO("Backup: Saving \"%s\" to \"%s\"... done!", databasePath_.c_str(), (databasePath_+".back").c_str());
01896
01897 ROS_INFO("Backup: Reloading memory...");
01898 rtabmap_.init(parameters_, databasePath_);
01899 ROS_INFO("Backup: Reloading memory... done!");
01900
01901 return true;
01902 }
01903
01904 bool CoreWrapper::setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01905 {
01906 ROS_INFO("rtabmap: Set localization mode");
01907 rtabmap::ParametersMap parameters;
01908 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
01909 rtabmap_.parseParameters(parameters);
01910 return true;
01911 }
01912
01913 bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01914 {
01915 ROS_INFO("rtabmap: Set mapping mode");
01916 rtabmap::ParametersMap parameters;
01917 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "true"));
01918 rtabmap_.parseParameters(parameters);
01919 return true;
01920 }
01921
01922 bool CoreWrapper::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01923 {
01924 ROS_INFO("rtabmap: Set log level to Debug");
01925 ULogger::setLevel(ULogger::kDebug);
01926 return true;
01927 }
01928 bool CoreWrapper::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01929 {
01930 ROS_INFO("rtabmap: Set log level to Info");
01931 ULogger::setLevel(ULogger::kInfo);
01932 return true;
01933 }
01934 bool CoreWrapper::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01935 {
01936 ROS_INFO("rtabmap: Set log level to Warning");
01937 ULogger::setLevel(ULogger::kWarning);
01938 return true;
01939 }
01940 bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
01941 {
01942 ROS_INFO("rtabmap: Set log level to Error");
01943 ULogger::setLevel(ULogger::kError);
01944 return true;
01945 }
01946
01947 bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res)
01948 {
01949 ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...",
01950 req.global?"true":"false",
01951 req.optimized?"true":"false",
01952 req.graphOnly?"true":"false");
01953 std::map<int, Signature> signatures;
01954 std::map<int, Transform> poses;
01955 std::multimap<int, Link> constraints;
01956
01957 if(req.graphOnly)
01958 {
01959 rtabmap_.getGraph(
01960 poses,
01961 constraints,
01962 req.optimized,
01963 req.global,
01964 &signatures);
01965 }
01966 else
01967 {
01968 rtabmap_.get3DMap(
01969 signatures,
01970 poses,
01971 constraints,
01972 req.optimized,
01973 req.global);
01974 }
01975
01976
01977 rtabmap_ros::mapDataToROS(poses,
01978 constraints,
01979 signatures,
01980 mapToOdom_,
01981 res.data);
01982
01983 res.data.header.stamp = ros::Time::now();
01984 res.data.header.frame_id = mapFrameId_;
01985
01986 return true;
01987 }
01988
01989 bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
01990 {
01991 std::map<int, rtabmap::Transform> filteredPoses;
01992 filteredPoses = mapsManager_.updateMapCaches(
01993 rtabmap_.getLocalOptimizedPoses(),
01994 rtabmap_.getMemory(),
01995 false,
01996 true,
01997 false,
01998 false,
01999 false);
02000 if(filteredPoses.size())
02001 {
02002
02003 float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
02004 cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize);
02005
02006 if(!pixels.empty())
02007 {
02008
02009 res.map.info.resolution = gridCellSize;
02010 res.map.info.origin.position.x = 0.0;
02011 res.map.info.origin.position.y = 0.0;
02012 res.map.info.origin.position.z = 0.0;
02013 res.map.info.origin.orientation.x = 0.0;
02014 res.map.info.origin.orientation.y = 0.0;
02015 res.map.info.origin.orientation.z = 0.0;
02016 res.map.info.origin.orientation.w = 1.0;
02017
02018 res.map.info.width = pixels.cols;
02019 res.map.info.height = pixels.rows;
02020 res.map.info.origin.position.x = xMin;
02021 res.map.info.origin.position.y = yMin;
02022 res.map.data.resize(res.map.info.width * res.map.info.height);
02023
02024 memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
02025
02026 res.map.header.frame_id = mapFrameId_;
02027 res.map.header.stamp = ros::Time::now();
02028 return true;
02029 }
02030 }
02031 return false;
02032 }
02033
02034 bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
02035 {
02036 std::map<int, rtabmap::Transform> filteredPoses;
02037 filteredPoses = mapsManager_.updateMapCaches(
02038 rtabmap_.getLocalOptimizedPoses(),
02039 rtabmap_.getMemory(),
02040 false,
02041 false,
02042 true,
02043 false,
02044 false);
02045 if(filteredPoses.size())
02046 {
02047
02048 float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
02049 cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize);
02050
02051 if(!pixels.empty())
02052 {
02053
02054 res.map.info.resolution = gridCellSize;
02055 res.map.info.origin.position.x = 0.0;
02056 res.map.info.origin.position.y = 0.0;
02057 res.map.info.origin.position.z = 0.0;
02058 res.map.info.origin.orientation.x = 0.0;
02059 res.map.info.origin.orientation.y = 0.0;
02060 res.map.info.origin.orientation.z = 0.0;
02061 res.map.info.origin.orientation.w = 1.0;
02062
02063 res.map.info.width = pixels.cols;
02064 res.map.info.height = pixels.rows;
02065 res.map.info.origin.position.x = xMin;
02066 res.map.info.origin.position.y = yMin;
02067 res.map.data.resize(res.map.info.width * res.map.info.height);
02068
02069 memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
02070
02071 res.map.header.frame_id = mapFrameId_;
02072 res.map.header.stamp = ros::Time::now();
02073 return true;
02074 }
02075 }
02076 return false;
02077 }
02078
02079 bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res)
02080 {
02081 ROS_INFO("rtabmap: Publishing map...");
02082
02083 if(mapDataPub_.getNumSubscribers() ||
02084 (!req.graphOnly && mapsManager_.hasSubscribers()) ||
02085 (req.graphOnly && labelsPub_.getNumSubscribers()))
02086 {
02087 std::map<int, Transform> poses;
02088 std::multimap<int, Link> constraints;
02089 std::map<int, Signature > signatures;
02090
02091 if(req.graphOnly)
02092 {
02093 rtabmap_.getGraph(
02094 poses,
02095 constraints,
02096 req.optimized,
02097 req.global,
02098 &signatures);
02099 }
02100 else
02101 {
02102 rtabmap_.get3DMap(
02103 signatures,
02104 poses,
02105 constraints,
02106 req.optimized,
02107 req.global);
02108 }
02109
02110 if(poses.size() && poses.size() != signatures.size())
02111 {
02112 ROS_WARN("poses and signatures are not the same size!? %d vs %d", (int)poses.size(), (int)signatures.size());
02113 }
02114
02115 ros::Time now = ros::Time::now();
02116 if(mapDataPub_.getNumSubscribers())
02117 {
02118 rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
02119 msg->header.stamp = now;
02120 msg->header.frame_id = mapFrameId_;
02121
02122 rtabmap_ros::mapDataToROS(poses,
02123 constraints,
02124 signatures,
02125 mapToOdom_,
02126 *msg);
02127
02128 mapDataPub_.publish(msg);
02129 }
02130
02131 if(mapGraphPub_.getNumSubscribers())
02132 {
02133 rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
02134 msg->header.stamp = now;
02135 msg->header.frame_id = mapFrameId_;
02136
02137 rtabmap_ros::mapGraphToROS(poses,
02138 constraints,
02139 mapToOdom_,
02140 *msg);
02141
02142 mapGraphPub_.publish(msg);
02143 }
02144
02145 if(!req.graphOnly && mapsManager_.hasSubscribers())
02146 {
02147 std::map<int, Transform> filteredPoses;
02148 if(signatures.size())
02149 {
02150 filteredPoses = mapsManager_.updateMapCaches(
02151 poses,
02152 rtabmap_.getMemory(),
02153 false,
02154 false,
02155 false,
02156 false,
02157 false,
02158 signatures);
02159 }
02160 else
02161 {
02162 filteredPoses = mapsManager_.getFilteredPoses(poses);
02163 }
02164 mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
02165 }
02166
02167 if(labelsPub_.getNumSubscribers())
02168 {
02169 if(poses.size() && signatures.size())
02170 {
02171 visualization_msgs::MarkerArray markers;
02172 for(std::map<int, Signature>::const_iterator iter=signatures.begin();
02173 iter!=signatures.end();
02174 ++iter)
02175 {
02176 std::map<int, Transform>::const_iterator poseIter= poses.find(iter->first);
02177 if(poseIter!=poses.end())
02178 {
02179
02180 if(!iter->second.getLabel().empty())
02181 {
02182 visualization_msgs::Marker marker;
02183 marker.header.frame_id = mapFrameId_;
02184 marker.header.stamp = now;
02185 marker.ns = "labels";
02186 marker.id = -iter->first;
02187 marker.action = visualization_msgs::Marker::ADD;
02188 marker.pose.position.x = poseIter->second.x();
02189 marker.pose.position.y = poseIter->second.y();
02190 marker.pose.position.z = poseIter->second.z();
02191 marker.pose.orientation.x = 0.0;
02192 marker.pose.orientation.y = 0.0;
02193 marker.pose.orientation.z = 0.0;
02194 marker.pose.orientation.w = 1.0;
02195 marker.scale.x = 1;
02196 marker.scale.y = 1;
02197 marker.scale.z = 0.5;
02198 marker.color.a = 0.7;
02199 marker.color.r = 1.0;
02200 marker.color.g = 0.0;
02201 marker.color.b = 0.0;
02202
02203 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
02204 marker.text = iter->second.getLabel();
02205
02206 markers.markers.push_back(marker);
02207 }
02208
02209 visualization_msgs::Marker marker;
02210 marker.header.frame_id = mapFrameId_;
02211 marker.header.stamp = now;
02212 marker.ns = "ids";
02213 marker.id = iter->first;
02214 marker.action = visualization_msgs::Marker::ADD;
02215 marker.pose.position.x = poseIter->second.x();
02216 marker.pose.position.y = poseIter->second.y();
02217 marker.pose.position.z = poseIter->second.z();
02218 marker.pose.orientation.x = 0.0;
02219 marker.pose.orientation.y = 0.0;
02220 marker.pose.orientation.z = 0.0;
02221 marker.pose.orientation.w = 1.0;
02222 marker.scale.x = 1;
02223 marker.scale.y = 1;
02224 marker.scale.z = 0.2;
02225 marker.color.a = 0.5;
02226 marker.color.r = 1.0;
02227 marker.color.g = 1.0;
02228 marker.color.b = 1.0;
02229 marker.lifetime = ros::Duration(2.0f/rate_);
02230
02231 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
02232 marker.text = uNumber2Str(iter->first);
02233
02234 markers.markers.push_back(marker);
02235 }
02236 }
02237
02238 if(markers.markers.size())
02239 {
02240 labelsPub_.publish(markers);
02241 }
02242 }
02243 }
02244 }
02245 else
02246 {
02247 UWARN("No subscribers, don't need to publish!");
02248 }
02249
02250 return true;
02251 }
02252
02253 bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
02254 {
02255 double planningTime = 0.0;
02256 goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now(), &planningTime);
02257 const std::vector<std::pair<int, Transform> > & path = rtabmap_.getPath();
02258 res.path_ids.resize(path.size());
02259 res.path_poses.resize(path.size());
02260 res.planning_time = planningTime;
02261 for(unsigned int i=0; i<path.size(); ++i)
02262 {
02263 res.path_ids[i] = path[i].first;
02264 rtabmap_ros::transformToPoseMsg(path[i].second, res.path_poses[i]);
02265 }
02266 return true;
02267 }
02268
02269 bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
02270 {
02271 if(rtabmap_.getPath().size())
02272 {
02273 ROS_WARN("Goal cancelled!");
02274 rtabmap_.clearPath(0);
02275 currentMetricGoal_.setNull();
02276 latestNodeWasReached_ = false;
02277 if(goalReachedPub_.getNumSubscribers())
02278 {
02279 std_msgs::Bool result;
02280 result.data = false;
02281 goalReachedPub_.publish(result);
02282 }
02283 }
02284 if(mbClient_.isServerConnected())
02285 {
02286 mbClient_.cancelGoal();
02287 }
02288
02289 return true;
02290 }
02291
02292 bool CoreWrapper::setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res)
02293 {
02294 if(rtabmap_.labelLocation(req.node_id, req.node_label))
02295 {
02296 if(req.node_id > 0)
02297 {
02298 ROS_INFO("Set label \"%s\" to node %d", req.node_label.c_str(), req.node_id);
02299 }
02300 else
02301 {
02302 ROS_INFO("Set label \"%s\" to last node", req.node_label.c_str());
02303 }
02304 }
02305 else
02306 {
02307 if(req.node_id > 0)
02308 {
02309 ROS_ERROR("Could not set label \"%s\" to node %d", req.node_label.c_str(), req.node_id);
02310 }
02311 else
02312 {
02313 ROS_ERROR("Could not set label \"%s\" to last node", req.node_label.c_str());
02314 }
02315 }
02316 return true;
02317 }
02318
02319 bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res)
02320 {
02321 if(rtabmap_.getMemory())
02322 {
02323 std::map<int, std::string> labels = rtabmap_.getMemory()->getAllLabels();
02324 res.labels = uValues(labels);
02325 ROS_INFO("List labels service: %d labels found.", (int)res.labels.size());
02326 }
02327 return true;
02328 }
02329
02330 void CoreWrapper::publishStats(const ros::Time & stamp)
02331 {
02332 UDEBUG("Publishing stats...");
02333 const rtabmap::Statistics & stats = rtabmap_.getStatistics();
02334
02335 if(infoPub_.getNumSubscribers())
02336 {
02337
02338 rtabmap_ros::InfoPtr msg(new rtabmap_ros::Info);
02339 msg->header.stamp = stamp;
02340 msg->header.frame_id = mapFrameId_;
02341
02342 rtabmap_ros::infoToROS(stats, *msg);
02343 infoPub_.publish(msg);
02344 }
02345
02346 if(mapDataPub_.getNumSubscribers())
02347 {
02348 rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
02349 msg->header.stamp = stamp;
02350 msg->header.frame_id = mapFrameId_;
02351
02352 rtabmap_ros::mapDataToROS(
02353 stats.poses(),
02354 stats.constraints(),
02355 stats.getSignatures(),
02356 stats.mapCorrection(),
02357 *msg);
02358
02359 mapDataPub_.publish(msg);
02360 }
02361
02362 if(mapGraphPub_.getNumSubscribers())
02363 {
02364 rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
02365 msg->header.stamp = stamp;
02366 msg->header.frame_id = mapFrameId_;
02367
02368 rtabmap_ros::mapGraphToROS(
02369 stats.poses(),
02370 stats.constraints(),
02371 stats.mapCorrection(),
02372 *msg);
02373
02374 mapGraphPub_.publish(msg);
02375 }
02376
02377 if(labelsPub_.getNumSubscribers())
02378 {
02379 if(stats.poses().size() && stats.getSignatures().size())
02380 {
02381 visualization_msgs::MarkerArray markers;
02382 for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
02383 iter!=stats.getSignatures().end();
02384 ++iter)
02385 {
02386 std::map<int, Transform>::const_iterator poseIter= stats.poses().find(iter->first);
02387 if(poseIter!=stats.poses().end())
02388 {
02389
02390 if(!iter->second.getLabel().empty())
02391 {
02392 visualization_msgs::Marker marker;
02393 marker.header.frame_id = mapFrameId_;
02394 marker.header.stamp = stamp;
02395 marker.ns = "labels";
02396 marker.id = -iter->first;
02397 marker.action = visualization_msgs::Marker::ADD;
02398 marker.pose.position.x = poseIter->second.x();
02399 marker.pose.position.y = poseIter->second.y();
02400 marker.pose.position.z = poseIter->second.z();
02401 marker.pose.orientation.x = 0.0;
02402 marker.pose.orientation.y = 0.0;
02403 marker.pose.orientation.z = 0.0;
02404 marker.pose.orientation.w = 1.0;
02405 marker.scale.x = 1;
02406 marker.scale.y = 1;
02407 marker.scale.z = 0.5;
02408 marker.color.a = 0.7;
02409 marker.color.r = 1.0;
02410 marker.color.g = 0.0;
02411 marker.color.b = 0.0;
02412
02413 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
02414 marker.text = iter->second.getLabel();
02415
02416 markers.markers.push_back(marker);
02417 }
02418
02419 visualization_msgs::Marker marker;
02420 marker.header.frame_id = mapFrameId_;
02421 marker.header.stamp = stamp;
02422 marker.ns = "ids";
02423 marker.id = iter->first;
02424 marker.action = visualization_msgs::Marker::ADD;
02425 marker.pose.position.x = poseIter->second.x();
02426 marker.pose.position.y = poseIter->second.y();
02427 marker.pose.position.z = poseIter->second.z();
02428 marker.pose.orientation.x = 0.0;
02429 marker.pose.orientation.y = 0.0;
02430 marker.pose.orientation.z = 0.0;
02431 marker.pose.orientation.w = 1.0;
02432 marker.scale.x = 1;
02433 marker.scale.y = 1;
02434 marker.scale.z = 0.2;
02435 marker.color.a = 0.5;
02436 marker.color.r = 1.0;
02437 marker.color.g = 1.0;
02438 marker.color.b = 1.0;
02439 marker.lifetime = ros::Duration(2.0f/rate_);
02440
02441 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
02442 marker.text = uNumber2Str(iter->first);
02443
02444 markers.markers.push_back(marker);
02445 }
02446 }
02447
02448 if(markers.markers.size())
02449 {
02450 labelsPub_.publish(markers);
02451 }
02452 }
02453 }
02454 }
02455
02456 void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
02457 {
02458 if(!currentMetricGoal_.isNull())
02459 {
02460 ROS_INFO("Publishing next goal: %d -> %s",
02461 rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str());
02462
02463 geometry_msgs::PoseStamped poseMsg;
02464 poseMsg.header.frame_id = mapFrameId_;
02465 poseMsg.header.stamp = stamp;
02466 rtabmap_ros::transformToPoseMsg(currentMetricGoal_, poseMsg.pose);
02467
02468 if(useActionForGoal_)
02469 {
02470 if(!mbClient_.isServerConnected())
02471 {
02472 ROS_INFO("Connecting to move_base action server...");
02473 mbClient_.waitForServer(ros::Duration(5.0));
02474 }
02475 if(mbClient_.isServerConnected())
02476 {
02477 move_base_msgs::MoveBaseGoal goal;
02478 goal.target_pose = poseMsg;
02479
02480 mbClient_.sendGoal(goal,
02481 boost::bind(&CoreWrapper::goalDoneCb, this, _1, _2),
02482 boost::bind(&CoreWrapper::goalActiveCb, this),
02483 boost::bind(&CoreWrapper::goalFeedbackCb, this, _1));
02484 }
02485 else
02486 {
02487 ROS_ERROR("Cannot connect to move_base action server!");
02488 }
02489 }
02490 if(nextMetricGoalPub_.getNumSubscribers())
02491 {
02492 nextMetricGoalPub_.publish(poseMsg);
02493 }
02494 }
02495 }
02496
02497
02498 void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
02499 const move_base_msgs::MoveBaseResultConstPtr& result)
02500 {
02501 bool ignore = false;
02502 if(!currentMetricGoal_.isNull())
02503 {
02504 if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
02505 {
02506 if(rtabmap_.getPath().size() &&
02507 rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first &&
02508 (!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first) || !latestNodeWasReached_))
02509 {
02510 ROS_WARN("Planning: move_base reached current goal but it is not "
02511 "the last one planned by rtabmap. A new goal should be sent when "
02512 "rtabmap will be able to retrieve next locations on the path.");
02513 ignore = true;
02514 }
02515 else
02516 {
02517 ROS_INFO("Planning: move_base success!");
02518 }
02519 }
02520 else
02521 {
02522 ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
02523 }
02524
02525 if(!ignore && goalReachedPub_.getNumSubscribers())
02526 {
02527 std_msgs::Bool result;
02528 result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;
02529 goalReachedPub_.publish(result);
02530 }
02531 }
02532
02533 if(!ignore)
02534 {
02535 rtabmap_.clearPath(1);
02536 currentMetricGoal_.setNull();
02537 latestNodeWasReached_ = false;
02538 }
02539 }
02540
02541
02542 void CoreWrapper::goalActiveCb()
02543 {
02544
02545 }
02546
02547
02548 void CoreWrapper::goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
02549 {
02550
02551
02552 }
02553
02554 void CoreWrapper::publishLocalPath(const ros::Time & stamp)
02555 {
02556 if(rtabmap_.getPath().size())
02557 {
02558 std::vector<std::pair<int, Transform> > poses = rtabmap_.getPathNextPoses();
02559 if(poses.size())
02560 {
02561 if(localPathPub_.getNumSubscribers())
02562 {
02563 nav_msgs::Path path;
02564 path.header.frame_id = mapFrameId_;
02565 path.header.stamp = stamp;
02566 path.poses.resize(poses.size());
02567 int oi = 0;
02568 for(std::vector<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02569 {
02570 path.poses[oi].header = path.header;
02571 rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
02572 ++oi;
02573 }
02574 localPathPub_.publish(path);
02575 }
02576 }
02577 }
02578 }
02579
02580 void CoreWrapper::publishGlobalPath(const ros::Time & stamp)
02581 {
02582 if(globalPathPub_.getNumSubscribers() && rtabmap_.getPath().size())
02583 {
02584 Transform pose = uValue(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPathCurrentGoalId(), Transform());
02585 if(!pose.isNull() && rtabmap_.getPathCurrentGoalIndex() < rtabmap_.getPath().size())
02586 {
02587
02588 Transform t = pose * rtabmap_.getPath().at(rtabmap_.getPathCurrentGoalIndex()).second.inverse();
02589
02590 nav_msgs::Path path;
02591 path.header.frame_id = mapFrameId_;
02592 path.header.stamp = stamp;
02593 path.poses.resize(rtabmap_.getPath().size());
02594 int oi = 0;
02595 for(std::vector<std::pair<int, Transform> >::const_iterator iter=rtabmap_.getPath().begin(); iter!=rtabmap_.getPath().end(); ++iter)
02596 {
02597 path.poses[oi].header = path.header;
02598 rtabmap_ros::transformToPoseMsg(t*iter->second, path.poses[oi].pose);
02599 ++oi;
02600 }
02601 if(!rtabmap_.getPathTransformToGoal().isIdentity())
02602 {
02603 path.poses.resize(path.poses.size()+1);
02604 Transform p = t * rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal();
02605 rtabmap_ros::transformToPoseMsg(p, path.poses[path.poses.size()-1].pose);
02606 }
02607 globalPathPub_.publish(path);
02608 }
02609 }
02610 }
02611
02612 #ifdef WITH_OCTOMAP_ROS
02613 #ifdef RTABMAP_OCTOMAP
02614 bool CoreWrapper::octomapBinaryCallback(
02615 octomap_msgs::GetOctomap::Request &req,
02616 octomap_msgs::GetOctomap::Response &res)
02617 {
02618 ROS_INFO("Sending binary map data on service request");
02619 res.map.header.frame_id = mapFrameId_;
02620 res.map.header.stamp = ros::Time::now();
02621
02622 std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
02623 poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true);
02624
02625 const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
02626 bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
02627 return success;
02628 }
02629
02630 bool CoreWrapper::octomapFullCallback(
02631 octomap_msgs::GetOctomap::Request &req,
02632 octomap_msgs::GetOctomap::Response &res)
02633 {
02634 ROS_INFO("Sending full map data on service request");
02635 res.map.header.frame_id = mapFrameId_;
02636 res.map.header.stamp = ros::Time::now();
02637
02638 std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
02639 poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true);
02640
02641 const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
02642 bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
02643 return success;
02644 }
02645 #endif
02646 #endif
02647
02659 void CoreWrapper::setupCallbacks(
02660 bool subscribeDepth,
02661 bool subscribeScan2d,
02662 bool subscribeScan3d,
02663 bool subscribeStereo,
02664 int queueSize,
02665 bool approxSync,
02666 int depthCameras)
02667 {
02668 ros::NodeHandle nh;
02669 ros::NodeHandle pnh("~");
02670
02671 if(subscribeDepth)
02672 {
02673 UASSERT(depthCameras >= 1 && depthCameras <= 2);
02674 UASSERT_MSG(depthCameras == 1 || !(subscribeScan2d || subscribeScan3d || !odomFrameId_.empty()), "Not yet supported!");
02675
02676 imageSubs_.resize(depthCameras);
02677 imageDepthSubs_.resize(depthCameras);
02678 cameraInfoSubs_.resize(depthCameras);
02679 for(int i=0; i<depthCameras; ++i)
02680 {
02681 std::string rgbPrefix = "rgb";
02682 std::string depthPrefix = "depth";
02683 if(depthCameras>1)
02684 {
02685 rgbPrefix += uNumber2Str(i);
02686 depthPrefix += uNumber2Str(i);
02687 }
02688 ros::NodeHandle rgb_nh(nh, rgbPrefix);
02689 ros::NodeHandle depth_nh(nh, depthPrefix);
02690 ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
02691 ros::NodeHandle depth_pnh(pnh, depthPrefix);
02692 image_transport::ImageTransport rgb_it(rgb_nh);
02693 image_transport::ImageTransport depth_it(depth_nh);
02694 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
02695 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
02696
02697 imageSubs_[i] = new image_transport::SubscriberFilter;
02698 imageDepthSubs_[i] = new image_transport::SubscriberFilter;
02699 cameraInfoSubs_[i] = new message_filters::Subscriber<sensor_msgs::CameraInfo>;
02700 imageSubs_[i]->subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
02701 imageDepthSubs_[i]->subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
02702 cameraInfoSubs_[i]->subscribe(rgb_nh, "camera_info", 1);
02703 }
02704
02705 if(odomFrameId_.empty())
02706 {
02707 odomSub_.subscribe(nh, "odom", 1);
02708 if(subscribeScan2d)
02709 {
02710 scanSub_.subscribe(nh, "scan", 1);
02711 depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
02712 MyDepthScanSyncPolicy(queueSize),
02713 *imageSubs_[0],
02714 odomSub_,
02715 *imageDepthSubs_[0],
02716 *cameraInfoSubs_[0],
02717 scanSub_);
02718 depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
02719
02720 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
02721 ros::this_node::getName().c_str(),
02722 imageSubs_[0]->getTopic().c_str(),
02723 imageDepthSubs_[0]->getTopic().c_str(),
02724 cameraInfoSubs_[0]->getTopic().c_str(),
02725 odomSub_.getTopic().c_str(),
02726 scanSub_.getTopic().c_str());
02727 }
02728 else if(subscribeScan3d)
02729 {
02730 scan3dSub_.subscribe(nh, "scan_cloud", 1);
02731 depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
02732 MyDepthScan3dSyncPolicy(queueSize),
02733 *imageSubs_[0],
02734 odomSub_,
02735 *imageDepthSubs_[0],
02736 *cameraInfoSubs_[0],
02737 scan3dSub_);
02738 depthScan3dSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dCallback, this, _1, _2, _3, _4, _5));
02739
02740 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
02741 ros::this_node::getName().c_str(),
02742 imageSubs_[0]->getTopic().c_str(),
02743 imageDepthSubs_[0]->getTopic().c_str(),
02744 cameraInfoSubs_[0]->getTopic().c_str(),
02745 odomSub_.getTopic().c_str(),
02746 scan3dSub_.getTopic().c_str());
02747 }
02748 else
02749 {
02750 if(depthCameras > 1)
02751 {
02752 depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
02753 MyDepth2SyncPolicy(queueSize),
02754 odomSub_,
02755 *imageSubs_[0],
02756 *imageDepthSubs_[0],
02757 *cameraInfoSubs_[0],
02758 *imageSubs_[1],
02759 *imageDepthSubs_[1],
02760 *cameraInfoSubs_[1]);
02761 depth2Sync_->registerCallback(boost::bind(&CoreWrapper::depth2Callback, this, _1, _2, _3, _4, _5, _6, _7));
02762
02763 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s\n %s,\n %s,\n %s",
02764 ros::this_node::getName().c_str(),
02765 imageSubs_[0]->getTopic().c_str(),
02766 imageDepthSubs_[0]->getTopic().c_str(),
02767 cameraInfoSubs_[0]->getTopic().c_str(),
02768 imageSubs_[1]->getTopic().c_str(),
02769 imageDepthSubs_[1]->getTopic().c_str(),
02770 cameraInfoSubs_[1]->getTopic().c_str(),
02771 odomSub_.getTopic().c_str());
02772 }
02773 else
02774 {
02775 if(approxSync)
02776 {
02777 depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
02778 MyDepthSyncPolicy(queueSize),
02779 *imageSubs_[0],
02780 odomSub_,
02781 *imageDepthSubs_[0],
02782 *cameraInfoSubs_[0]);
02783 depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
02784 }
02785 else
02786 {
02787 depthExactSync_ = new message_filters::Synchronizer<MyDepthExactSyncPolicy>(
02788 MyDepthExactSyncPolicy(queueSize),
02789 *imageSubs_[0],
02790 odomSub_,
02791 *imageDepthSubs_[0],
02792 *cameraInfoSubs_[0]);
02793 depthExactSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
02794 }
02795 ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
02796 ros::this_node::getName().c_str(),
02797 approxSync?"approx":"exact",
02798 imageSubs_[0]->getTopic().c_str(),
02799 imageDepthSubs_[0]->getTopic().c_str(),
02800 cameraInfoSubs_[0]->getTopic().c_str(),
02801 odomSub_.getTopic().c_str());
02802 }
02803 }
02804 }
02805 else
02806 {
02807
02808 if(subscribeScan2d)
02809 {
02810 scanSub_.subscribe(nh, "scan", 1);
02811 depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
02812 MyDepthScanTFSyncPolicy(queueSize),
02813 *imageSubs_[0],
02814 *imageDepthSubs_[0],
02815 *cameraInfoSubs_[0],
02816 scanSub_);
02817 depthScanTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
02818
02819 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
02820 ros::this_node::getName().c_str(),
02821 imageSubs_[0]->getTopic().c_str(),
02822 imageDepthSubs_[0]->getTopic().c_str(),
02823 cameraInfoSubs_[0]->getTopic().c_str(),
02824 scanSub_.getTopic().c_str());
02825 }
02826 else if(subscribeScan3d)
02827 {
02828 scan3dSub_.subscribe(nh, "scan_cloud", 1);
02829 depthScan3dTFSync_ = new message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy>(
02830 MyDepthScan3dTFSyncPolicy(queueSize),
02831 *imageSubs_[0],
02832 *imageDepthSubs_[0],
02833 *cameraInfoSubs_[0],
02834 scan3dSub_);
02835 depthScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScan3dTFCallback, this, _1, _2, _3, _4));
02836
02837 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
02838 ros::this_node::getName().c_str(),
02839 imageSubs_[0]->getTopic().c_str(),
02840 imageDepthSubs_[0]->getTopic().c_str(),
02841 cameraInfoSubs_[0]->getTopic().c_str(),
02842 scan3dSub_.getTopic().c_str());
02843 }
02844 else
02845 {
02846 if(approxSync)
02847 {
02848 depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
02849 MyDepthTFSyncPolicy(queueSize),
02850 *imageSubs_[0],
02851 *imageDepthSubs_[0],
02852 *cameraInfoSubs_[0]);
02853 depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
02854 }
02855 else
02856 {
02857 depthTFExactSync_ = new message_filters::Synchronizer<MyDepthTFExactSyncPolicy>(
02858 MyDepthTFExactSyncPolicy(queueSize),
02859 *imageSubs_[0],
02860 *imageDepthSubs_[0],
02861 *cameraInfoSubs_[0]);
02862 depthTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
02863 }
02864 ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
02865 ros::this_node::getName().c_str(),
02866 approxSync?"approx":"exact",
02867 imageSubs_[0]->getTopic().c_str(),
02868 imageDepthSubs_[0]->getTopic().c_str(),
02869 cameraInfoSubs_[0]->getTopic().c_str());
02870 }
02871 }
02872 }
02873 else if(subscribeStereo)
02874 {
02875 ros::NodeHandle left_nh(nh, "left");
02876 ros::NodeHandle right_nh(nh, "right");
02877 ros::NodeHandle left_pnh(pnh, "left");
02878 ros::NodeHandle right_pnh(pnh, "right");
02879 image_transport::ImageTransport left_it(left_nh);
02880 image_transport::ImageTransport right_it(right_nh);
02881 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
02882 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
02883
02884 imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
02885 imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
02886 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
02887 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
02888
02889 if(odomFrameId_.empty())
02890 {
02891 odomSub_.subscribe(nh, "odom", 1);
02892 if(subscribeScan2d)
02893 {
02894 scanSub_.subscribe(nh, "scan", 1);
02895 stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
02896 MyStereoScanSyncPolicy(queueSize),
02897 imageRectLeft_,
02898 imageRectRight_,
02899 cameraInfoLeft_,
02900 cameraInfoRight_,
02901 scanSub_,
02902 odomSub_);
02903 stereoScanSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
02904
02905 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
02906 ros::this_node::getName().c_str(),
02907 imageRectLeft_.getTopic().c_str(),
02908 imageRectRight_.getTopic().c_str(),
02909 cameraInfoLeft_.getTopic().c_str(),
02910 cameraInfoRight_.getTopic().c_str(),
02911 odomSub_.getTopic().c_str(),
02912 scanSub_.getTopic().c_str());
02913 }
02914 else if(subscribeScan3d)
02915 {
02916 scan3dSub_.subscribe(nh, "scan_cloud", 1);
02917 stereoScan3dSync_ = new message_filters::Synchronizer<MyStereoScan3dSyncPolicy>(
02918 MyStereoScan3dSyncPolicy(queueSize),
02919 imageRectLeft_,
02920 imageRectRight_,
02921 cameraInfoLeft_,
02922 cameraInfoRight_,
02923 scan3dSub_,
02924 odomSub_);
02925 stereoScan3dSync_->registerCallback(boost::bind(&CoreWrapper::stereoScan3dCallback, this, _1, _2, _3, _4, _5, _6));
02926
02927 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
02928 ros::this_node::getName().c_str(),
02929 imageRectLeft_.getTopic().c_str(),
02930 imageRectRight_.getTopic().c_str(),
02931 cameraInfoLeft_.getTopic().c_str(),
02932 cameraInfoRight_.getTopic().c_str(),
02933 odomSub_.getTopic().c_str(),
02934 scan3dSub_.getTopic().c_str());
02935 }
02936 else
02937 {
02938 if(approxSync)
02939 {
02940 stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(
02941 MyStereoApproxSyncPolicy(queueSize),
02942 imageRectLeft_,
02943 imageRectRight_,
02944 cameraInfoLeft_,
02945 cameraInfoRight_,
02946 odomSub_);
02947 stereoApproxSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
02948 }
02949 else
02950 {
02951 stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(
02952 MyStereoExactSyncPolicy(queueSize),
02953 imageRectLeft_,
02954 imageRectRight_,
02955 cameraInfoLeft_,
02956 cameraInfoRight_,
02957 odomSub_);
02958 stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
02959 }
02960
02961 ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s",
02962 ros::this_node::getName().c_str(),
02963 approxSync?"approx":"exact",
02964 imageRectLeft_.getTopic().c_str(),
02965 imageRectRight_.getTopic().c_str(),
02966 cameraInfoLeft_.getTopic().c_str(),
02967 cameraInfoRight_.getTopic().c_str(),
02968 odomSub_.getTopic().c_str());
02969 }
02970 }
02971 else
02972 {
02973
02974 if(subscribeScan2d)
02975 {
02976 scanSub_.subscribe(nh, "scan", 1);
02977 stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
02978 MyStereoScanTFSyncPolicy(queueSize),
02979 imageRectLeft_,
02980 imageRectRight_,
02981 cameraInfoLeft_,
02982 cameraInfoRight_,
02983 scanSub_);
02984 stereoScanTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanTFCallback, this, _1, _2, _3, _4, _5));
02985
02986 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
02987 ros::this_node::getName().c_str(),
02988 imageRectLeft_.getTopic().c_str(),
02989 imageRectRight_.getTopic().c_str(),
02990 cameraInfoLeft_.getTopic().c_str(),
02991 cameraInfoRight_.getTopic().c_str(),
02992 scanSub_.getTopic().c_str());
02993 }
02994 else if(subscribeScan3d)
02995 {
02996 scan3dSub_.subscribe(nh, "scan_cloud", 1);
02997 stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
02998 MyStereoScan3dTFSyncPolicy(queueSize),
02999 imageRectLeft_,
03000 imageRectRight_,
03001 cameraInfoLeft_,
03002 cameraInfoRight_,
03003 scan3dSub_);
03004 stereoScan3dTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScan3dTFCallback, this, _1, _2, _3, _4, _5));
03005
03006 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
03007 ros::this_node::getName().c_str(),
03008 imageRectLeft_.getTopic().c_str(),
03009 imageRectRight_.getTopic().c_str(),
03010 cameraInfoLeft_.getTopic().c_str(),
03011 cameraInfoRight_.getTopic().c_str(),
03012 scan3dSub_.getTopic().c_str());
03013 }
03014 else
03015 {
03016 if(approxSync)
03017 {
03018 stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(
03019 MyStereoApproxTFSyncPolicy(queueSize),
03020 imageRectLeft_,
03021 imageRectRight_,
03022 cameraInfoLeft_,
03023 cameraInfoRight_);
03024 stereoApproxTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
03025 }
03026 else
03027 {
03028 stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(
03029 MyStereoExactTFSyncPolicy(queueSize),
03030 imageRectLeft_,
03031 imageRectRight_,
03032 cameraInfoLeft_,
03033 cameraInfoRight_);
03034 stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
03035 }
03036
03037 ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
03038 ros::this_node::getName().c_str(),
03039 approxSync?"approx":"exact",
03040 imageRectLeft_.getTopic().c_str(),
03041 imageRectRight_.getTopic().c_str(),
03042 cameraInfoLeft_.getTopic().c_str(),
03043 cameraInfoRight_.getTopic().c_str());
03044 }
03045 }
03046 }
03047 else
03048 {
03049 ros::NodeHandle rgb_nh(nh, "rgb");
03050 ros::NodeHandle rgb_pnh(pnh, "rgb");
03051 image_transport::ImageTransport rgb_it(rgb_nh);
03052 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
03053 defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
03054
03055 ROS_INFO("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), defaultSub_.getTopic().c_str());
03056 }
03057 }
03058
03059
03060