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