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