diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c6614ec..76b532dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(rviz) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(RTABMap 0.11.9 REQUIRED) +find_package(RTABMap 0.11.10 REQUIRED) find_package(OpenCV REQUIRED) diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h index a4421773..c6d6e7d5 100644 --- a/include/rtabmap_ros/CoreWrapper.h +++ b/include/rtabmap_ros/CoreWrapper.h @@ -77,6 +77,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include typedef actionlib::SimpleActionClient MoveBaseClient; +namespace rtabmap { +class StereoDense; +} + class CoreWrapper { public: @@ -226,7 +230,8 @@ private: bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&); - bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res); + bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res); + bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&); @@ -239,7 +244,7 @@ private: bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res); #endif - rtabmap::ParametersMap loadParameters(const std::string & configFile); + void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters); void saveParameters(const std::string & configFile); void publishLoop(double tfDelay, double tfTolerance); @@ -489,6 +494,7 @@ private: ros::ServiceServer setLogErrorSrv_; ros::ServiceServer getMapDataSrv_; ros::ServiceServer getProjMapSrv_; + ros::ServiceServer getMapSrv_; ros::ServiceServer getGridMapSrv_; ros::ServiceServer publishMapDataSrv_; ros::ServiceServer setGoalSrv_; @@ -504,6 +510,7 @@ private: boost::thread* transformThread_; + bool stereoToDepth_; float rate_; bool createIntermediateNodes_; ros::Time time_; diff --git a/include/rtabmap_ros/MapsManager.h b/include/rtabmap_ros/MapsManager.h index 6c1cb72e..f665bb69 100644 --- a/include/rtabmap_ros/MapsManager.h +++ b/include/rtabmap_ros/MapsManager.h @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define MAPSMANAGER_H_ #include +#include #include #include #include @@ -37,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { class OctoMap; class Memory; +class OccupancyGrid; } // namespace rtabmap @@ -46,6 +48,8 @@ public: virtual ~MapsManager(); void clear(); bool hasSubscribers() const; + void backwardCompatibilityParameters(rtabmap::ParametersMap & parameters) const; + void setParameters(const rtabmap::ParametersMap & parameters); std::map getFilteredPoses( const std::map & poses); @@ -53,10 +57,7 @@ public: std::map updateMapCaches( const std::map & poses, const rtabmap::Memory * memory, - bool updateCloud, - bool updateProj, bool updateGrid, - bool updateScan, bool updateOctomap, const std::map & signatures = std::map()); @@ -65,12 +66,6 @@ public: const ros::Time & stamp, const std::string & mapFrameId); - cv::Mat generateProjMap( - const std::map & filteredPoses, - float & xMin, - float & yMin, - float & gridCellSize); - cv::Mat generateGridMap( const std::map & filteredPoses, float & xMin, @@ -81,37 +76,20 @@ public: private: // mapping stuff - int cloudDecimation_; - double cloudMaxDepth_; - double cloudMinDepth_; - double cloudVoxelSize_; - double cloudFloorCullingHeight_; - double cloudCeilingCullingHeight_; bool cloudOutputVoxelized_; - bool cloudFrustumCulling_; - double cloudNoiseFilteringRadius_; - int cloudNoiseFilteringMinNeighbors_; - int scanDecimation_; - double scanVoxelSize_; - bool scanOutputVoxelized_; - double projMaxGroundAngle_; - int projMinClusterSize_; - double projMaxObstaclesHeight_; - double projMaxGroundHeight_; - bool projDetectFlatObstacles_; - bool projMapFrame_; double gridCellSize_; + bool gridIncremental_; double gridSize_; bool gridEroded_; double footprintRadius_; - bool gridUnknownSpaceFilled_; - double gridMaxUnknownSpaceFilledRange_; double mapFilterRadius_; double mapFilterAngle_; bool mapCacheCleanup_; bool negativePosesIgnored_; ros::Publisher cloudMapPub_; + ros::Publisher cloudGroundPub_; + ros::Publisher cloudObstaclesPub_; ros::Publisher projMapPub_; ros::Publisher gridMapPub_; ros::Publisher scanMapPub_; @@ -121,15 +99,22 @@ private: ros::Publisher octoMapEmptySpace_; ros::Publisher octoMapProj_; - std::map::Ptr > clouds_; - std::map::Ptr > scans_; - std::map > cameraModels_; - std::map > projMaps_; // + std::map assembledGroundPoses_; + std::map assembledObstaclePoses_; + pcl::PointCloud::Ptr assembledObstacles_; + pcl::PointCloud::Ptr assembledGround_; + + std::map gridPoses_; + cv::Mat gridMap_; std::map > gridMaps_; // + std::map gridMapsViewpoints_; + + rtabmap::OccupancyGrid * occupancyGrid_; rtabmap::OctoMap * octomap_; int octomapTreeDepth_; - bool octomapGroundIsObstacle_; + + rtabmap::ParametersMap parameters_; }; #endif /* MAPSMANAGER_H_ */ diff --git a/msg/NodeData.msg b/msg/NodeData.msg index f9ce1d0d..ed560c50 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -33,11 +33,19 @@ geometry_msgs/Transform[] localTransform uint8[] laserScan int32 laserScanMaxPts float32 laserScanMaxRange +geometry_msgs/Transform laserScanLocalTransform # compressed user data # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" uint8[] userData +# compressed occupancy grid +# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +uint8[] grid_ground +uint8[] grid_obstacles +float32 grid_cell_size +Point3f grid_view_point + # std::multimap # std::multimap int32[] wordIds diff --git a/package.xml b/package.xml index c815632b..6eab9a07 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.11.9 + 0.11.10 RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/src/CoreNode.cpp b/src/CoreNode.cpp index df608f12..eacf6972 100644 --- a/src/CoreNode.cpp +++ b/src/CoreNode.cpp @@ -51,9 +51,8 @@ int main(int argc, char** argv) else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0) { rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters(); - uInsert(parameters, - std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(), - UDirectory::homeDir()+"/.ros")); // change default to ~/.ros + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros if(strcmp(argv[i], "--params") == 0) { diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index c18ca3cb..2cb8f85a 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -124,6 +125,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) stereoApproxTFSync_(0), stereoExactTFSync_(0), transformThread_(0), + stereoToDepth_(false), rate_(Parameters::defaultRtabmapDetectionRate()), createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()), time_(ros::Time::now()), @@ -208,6 +210,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_); pnh.param("flip_scan", flipScan_, flipScan_); + pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_); if(!tfPrefix.empty()) { @@ -285,12 +288,15 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) databasePath_ = UDirectory::currentDir(true) + databasePath_; } + ParametersMap allParameters = Parameters::getDefaultParameters(); + uInsert(allParameters, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS + uInsert(allParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros + // load parameters - parameters_ = loadParameters(configPath_); + loadParameters(configPath_, parameters_); // update parameters with user input parameters (private) - uInsert(parameters_, std::make_pair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros - for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) + for(ParametersMap::iterator iter=allParameters.begin(); iter!=allParameters.end(); ++iter) { std::string vStr; bool vBool; @@ -299,31 +305,31 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) if(pnh.getParam(iter->first, vStr)) { ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str()); - iter->second = vStr; if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0) { - iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir()); + vStr = uReplaceChar(vStr, '~', UDirectory::homeDir()); } else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0) { - iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir()); + vStr = uReplaceChar(vStr, '~', UDirectory::homeDir()); } + uInsert(parameters_, ParametersPair(iter->first, vStr)); } else if(pnh.getParam(iter->first, vBool)) { ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str()); - iter->second = uBool2Str(vBool); + uInsert(parameters_, ParametersPair(iter->first, uBool2Str(vBool))); } else if(pnh.getParam(iter->first, vDouble)) { ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str()); - iter->second = uNumber2Str(vDouble); + uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vDouble))); } else if(pnh.getParam(iter->first, vInt)) { ROS_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str()); - iter->second = uNumber2Str(vInt); + uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vInt))); } } @@ -345,7 +351,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) if(iter->second.first) { // can be migrated - parameters_.at(iter->second.second)= vStr; + uInsert(parameters_, ParametersPair(iter->second.second, vStr)); ROS_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.", iter->first.c_str(), iter->second.second.c_str(), vStr.c_str()); } @@ -365,6 +371,25 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) } } + // Backward compatibility (MapsManager) + mapsManager_.backwardCompatibilityParameters(parameters_); + + if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end()) + { + ROS_WARN("Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is " + "true. The occupancy grid map will be constructed from " + "laser scans. To get occupancy grid map from cloud projection, set \"%s\" " + "to true. To suppress this warning, " + "add ", + Parameters::kGridFromDepth().c_str(), + Parameters::kGridFromDepth().c_str(), + Parameters::kGridFromDepth().c_str()); + parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false")); + } + + // Add all other parameters (not copied if already exists) + parameters_.insert(allParameters.begin(), allParameters.end()); + // set public parameters nh.setParam("is_rtabmap_paused", paused_); for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) @@ -391,9 +416,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) if(!subscribeDepth && !subscribeStereo) { ROS_WARN("ROS param subscribe_depth and subscribe_stereo are false, but RTAB-Map " - "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth or subscribe_stereo " - "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure " - "detection on images-only."); + "parameter \"%s\" is true! Please set subscribe_depth or subscribe_stereo " + "to true to use rtabmap node for RGB-D SLAM, or set \"%s\" to false for loop closure " + "detection on images-only.", Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str()); } } @@ -419,6 +444,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) ROS_INFO("rtabmap: database_path parameter not set, the map will not be saved."); } + mapsManager_.setParameters(parameters_); + if(subscribeStereo) + { + ROS_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false"); + } + // Init RTAB-Map rtabmap_.init(parameters_, databasePath_); @@ -436,7 +467,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters) backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this); setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this); setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this); - getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); + getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this); + getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this); getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this); getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this); publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this); @@ -550,9 +582,8 @@ CoreWrapper::~CoreWrapper() printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str()); } -ParametersMap CoreWrapper::loadParameters(const std::string & configFile) +void CoreWrapper::loadParameters(const std::string & configFile, ParametersMap & parameters) { - ParametersMap parameters = Parameters::getDefaultParameters(); if(!configFile.empty()) { ROS_INFO("Loading parameters from %s", configFile.c_str()); @@ -562,9 +593,6 @@ ParametersMap CoreWrapper::loadParameters(const std::string & configFile) } Parameters::readINI(configFile.c_str(), parameters); } - // otherwise take default parameters - - return parameters; } void CoreWrapper::saveParameters(const std::string & configFile) @@ -993,12 +1021,14 @@ void CoreWrapper::commonDepthCallback( } cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, + scanLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, - scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull()) + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)); + if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec()); return; @@ -1007,7 +1037,7 @@ void CoreWrapper::commonDepthCallback( //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -1024,8 +1054,7 @@ void CoreWrapper::commonDepthCallback( } else { - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } @@ -1051,13 +1080,12 @@ void CoreWrapper::commonDepthCallback( } // sync with odometry stamp - Transform localScanTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); - if(localScanTransform.isNull()) + scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); + if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); return; } - Transform laserOdomT = localScanTransform; if(lastPoseStamp_ != scan3dMsg->header.stamp) { if(!odomT.isNull()) @@ -1070,7 +1098,7 @@ void CoreWrapper::commonDepthCallback( } else { - laserOdomT = odomT.inverse() * sensorT * localScanTransform; + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } @@ -1080,10 +1108,6 @@ void CoreWrapper::commonDepthCallback( { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*scan3dMsg, *pclScan); - if(!laserOdomT.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, laserOdomT); - } scan = util3d::laserScanFromPointCloud(*pclScan); } else @@ -1091,11 +1115,6 @@ void CoreWrapper::commonDepthCallback( pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*scan3dMsg, *pclScan); - if(!laserOdomT.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, laserOdomT); - } - if(scanCloudNormalK_ > 0) { //compute normals @@ -1126,8 +1145,10 @@ void CoreWrapper::commonDepthCallback( } SensorData data(scan, - scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0), - scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + LaserScanInfo( + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0), + scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f), + scanLocalTransform), rgb, depth, cameraModels, @@ -1167,6 +1188,77 @@ void CoreWrapper::commonStereoCallback( return; } + cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage; + if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || + leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) + { + ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8"); + } + else + { + ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8"); + } + ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8"); + + Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp); + if(localTransform.isNull()) + { + return; + } + + rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); + + if(stereoModel.baseline() > 10.0) + { + static bool shown = false; + if(!shown) + { + ROS_WARN("Detected baseline (%f m) is quite large! Is your " + "right camera_info P(0,3) correctly set? Note that " + "baseline=-P(0,3)/P(0,0). This warning is printed only once.", + stereoModel.baseline()); + shown = true; + } + } + + if(stereoToDepth_) + { + // cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters + cv::Mat disparity = util2d::disparityFromStereoImages(ptrLeftImage->image, ptrRightImage->image, parameters_); + if(disparity.empty()) + { + ROS_ERROR("Could not compute disparity image (\"stereo_to_depth\" is true)!"); + return; + } + cv::Mat depth = util2d::depthFromDisparity( + disparity, + stereoModel.left().fx(), + stereoModel.baseline()); + + if(depth.empty()) + { + ROS_ERROR("Could not compute depth image (\"stereo_to_depth\" is true)!"); + return; + } + UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); + + // move to common depth callback + cv_bridge::CvImage imgDepth; + if(depth.type() == CV_16UC1) + { + imgDepth.encoding = sensor_msgs::image_encodings::TYPE_16UC1; + } + else // CV_32FC1 + { + imgDepth.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + } + imgDepth.image = depth; + sensor_msgs::ImagePtr depthMsg = imgDepth.toImageMsg(); + depthMsg->header = leftImageMsg->header; + + commonDepthCallback(odomFrameId, leftImageMsg, depthMsg, leftCamInfoMsg, scan2dMsg, scan3dMsg); + } + //for sync transform Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_); if(odomT.isNull() && !odomFrameId_.empty()) @@ -1175,12 +1267,6 @@ void CoreWrapper::commonStereoCallback( odomFrameId.c_str(), frameId_.c_str()); } - Transform localTransform = getTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp); - if(localTransform.isNull()) - { - return; - } - // sync with odometry stamp if(lastPoseStamp_ != leftImageMsg->header.stamp) { @@ -1196,12 +1282,14 @@ void CoreWrapper::commonStereoCallback( } cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, + scanLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, - scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)).isNull()) + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)); + if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scan2dMsg->header.stamp.toSec()); return; @@ -1211,7 +1299,7 @@ void CoreWrapper::commonStereoCallback( sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; //projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfBuffer_); - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -1225,9 +1313,7 @@ void CoreWrapper::commonStereoCallback( { return; } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); - + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } @@ -1252,13 +1338,12 @@ void CoreWrapper::commonStereoCallback( } // sync with odometry stamp - Transform localScanTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); - if(localScanTransform.isNull()) + scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); + if(scanLocalTransform.isNull()) { ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); return; } - Transform laserOdomT = localScanTransform; if(lastPoseStamp_ != scan3dMsg->header.stamp) { if(!odomT.isNull()) @@ -1271,7 +1356,7 @@ void CoreWrapper::commonStereoCallback( } else { - laserOdomT = odomT.inverse() * sensorT * localScanTransform; + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } @@ -1282,11 +1367,6 @@ void CoreWrapper::commonStereoCallback( pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*scan3dMsg, *pclScan); - if(!laserOdomT.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, laserOdomT); - } - scan = util3d::laserScanFromPointCloud(*pclScan); } else @@ -1294,11 +1374,6 @@ void CoreWrapper::commonStereoCallback( pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*scan3dMsg, *pclScan); - if(!laserOdomT.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, laserOdomT); - } - if(scanCloudNormalK_ > 0) { //compute normals @@ -1314,33 +1389,6 @@ void CoreWrapper::commonStereoCallback( } } - cv_bridge::CvImagePtr ptrLeftImage, ptrRightImage; - if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 || - leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) - { - ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "mono8"); - } - else - { - ptrLeftImage = cv_bridge::toCvCopy(leftImageMsg, "bgr8"); - } - ptrRightImage = cv_bridge::toCvCopy(rightImageMsg, "mono8"); - - rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform); - - if(stereoModel.baseline() > 10.0) - { - static bool shown = false; - if(!shown) - { - ROS_WARN("Detected baseline (%f m) is quite large! Is your " - "right camera_info P(0,3) correctly set? Note that " - "baseline=-P(0,3)/P(0,0). This warning is printed only once.", - stereoModel.baseline()); - shown = true; - } - } - ros::Time stamp = scan2dMsg.get() != 0?scan2dMsg->header.stamp: scan3dMsg.get() != 0?scan3dMsg->header.stamp: leftImageMsg->header.stamp; @@ -1352,8 +1400,10 @@ void CoreWrapper::commonStereoCallback( } SensorData data(scan, - scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0, - scan2dMsg.get() != 0?scan2dMsg->range_max:0, + LaserScanInfo( + scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0, + scan2dMsg.get() != 0?scan2dMsg->range_max:0, + scanLocalTransform), ptrLeftImage->image, ptrRightImage->image, stereoModel, @@ -1621,9 +1671,6 @@ void CoreWrapper::process( rtabmap_.getMemory(), false, false, - false, - false, - false, tmpSignature); timeUpdateMaps = timer.ticks(); @@ -1916,6 +1963,7 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp ROS_INFO("RTAB-Map rate detection = %f Hz", rate_); } rtabmap_.parseParameters(parameters_); + mapsManager_.setParameters(parameters_); return true; } @@ -2039,7 +2087,7 @@ bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Respon return true; } -bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res) +bool CoreWrapper::getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res) { ROS_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...", req.global?"true":"false", @@ -2083,65 +2131,41 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros: bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { - std::map filteredPoses; - filteredPoses = mapsManager_.updateMapCaches( - rtabmap_.getLocalOptimizedPoses(), - rtabmap_.getMemory(), - false, - true, - false, - false, - false); - if(filteredPoses.size()) + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + !uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) { - // create the projection map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize); - - if(!pixels.empty()) - { - //init - res.map.info.resolution = gridCellSize; - res.map.info.origin.position.x = 0.0; - res.map.info.origin.position.y = 0.0; - res.map.info.origin.position.z = 0.0; - res.map.info.origin.orientation.x = 0.0; - res.map.info.origin.orientation.y = 0.0; - res.map.info.origin.orientation.z = 0.0; - res.map.info.origin.orientation.w = 1.0; - - res.map.info.width = pixels.cols; - res.map.info.height = pixels.rows; - res.map.info.origin.position.x = xMin; - res.map.info.origin.position.y = yMin; - res.map.data.resize(res.map.info.width * res.map.info.height); - - memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height); - - res.map.header.frame_id = mapFrameId_; - res.map.header.stamp = ros::Time::now(); - return true; - } + ROS_WARN("/get_proj_map service is deprecated! Call /get_grid_map service " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); } - return false; + else + { + ROS_WARN("/get_proj_map service is deprecated! Call /get_grid_map service instead."); + } + return getGridMapCallback(req, res); } bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) +{ + ROS_WARN("/get_grid_map service is deprecated! Call /get_map service instead."); + return getMapCallback(req, res); +} + +bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res) { std::map filteredPoses; filteredPoses = mapsManager_.updateMapCaches( rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory(), - false, - false, true, - false, false); if(filteredPoses.size()) { // create the grid map float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = mapsManager_.generateProjMap(filteredPoses, xMin, yMin, gridCellSize); + cv::Mat pixels = mapsManager_.generateGridMap(filteredPoses, xMin, yMin, gridCellSize); if(!pixels.empty()) { @@ -2247,9 +2271,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab rtabmap_.getMemory(), false, false, - false, - false, - false, signatures); } else @@ -2715,7 +2736,7 @@ bool CoreWrapper::octomapBinaryCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true); const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map); @@ -2731,7 +2752,7 @@ bool CoreWrapper::octomapFullCallback( res.map.header.stamp = ros::Time::now(); std::map poses = rtabmap_.getLocalOptimizedPoses(); - poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true); + poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true); const rtabmap::OctoMap * octomap = mapsManager_.getOctomap(); bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map); diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index a3f51128..51f1919c 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -407,9 +407,9 @@ void GuiWrapper::handleEvent(UEvent * anEvent) getMapSrv.request.global = cmdEvent->value1().toBool(); getMapSrv.request.optimized = cmdEvent->value2().toBool(); getMapSrv.request.graphOnly = cmdEvent->value3().toBool(); - if(!ros::service::call("get_map", getMapSrv)) + if(!ros::service::call("get_map_data", getMapSrv)) { - ROS_WARN("Can't call \"get_map\" service"); + ROS_WARN("Can't call \"get_map_data\" service"); this->post(new RtabmapEvent3DMap(1)); // service error } else @@ -588,6 +588,7 @@ void GuiWrapper::commonDepthCallback( cv::Mat depth; std::vector cameraModels; cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); rtabmap::OdometryInfo info; bool ignoreData = false; @@ -693,15 +694,20 @@ void GuiWrapper::commonDepthCallback( if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) + scanLocalTransform = getTransform( + frameId_, + scan2dMsg->header.frame_id, + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)); + if(scanLocalTransform.isNull()) { + ROS_ERROR("TF of received scan at time %fs is not set, aborting rtabmapviz update.", scan2dMsg->header.stamp.toSec()); return; } //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -715,18 +721,62 @@ void GuiWrapper::commonDepthCallback( { return; } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); - + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } scan = util3d::laserScanFromPointCloud(*pclScan); } else if(scan3dMsg.get() != 0) { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - scan = util3d::laserScanFromPointCloud(*pclScan); + bool containNormals = false; + for(unsigned int i=0; ifields.size(); ++i) + { + if(scan3dMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + + // sync with odometry stamp + scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); + if(scanLocalTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); + return; + } + if(odomHeader.stamp != scan3dMsg->header.stamp) + { + if(!odomT.isNull()) + { + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan3dMsg->header.stamp); + if(sensorT.isNull()) + { + ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry " + "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomHeader.stamp.toSec()); + } + else + { + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; + } + + } + } + + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = util3d::laserScanFromPointCloud(*pclScan); + } } if(odomInfoMsg.get()) @@ -749,8 +799,10 @@ void GuiWrapper::commonDepthCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get()?(int)scan2dMsg->range_max:0, + LaserScanInfo( + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, + scanLocalTransform), rgb, depth, cameraModels, @@ -854,6 +906,7 @@ void GuiWrapper::commonStereoCallback( cv::Mat left; cv::Mat right; cv::Mat scan; + Transform scanLocalTransform = Transform::getIdentity(); rtabmap::StereoCameraModel stereoModel; rtabmap::OdometryInfo info; bool ignoreData = false; @@ -898,15 +951,20 @@ void GuiWrapper::commonStereoCallback( if(scan2dMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp).isNull()) + scanLocalTransform = getTransform( + frameId_, + scan2dMsg->header.frame_id, + scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment)); + if(scanLocalTransform.isNull()) { + ROS_ERROR("TF of received scan at time %fs is not set, aborting rtabmapviz update.", scan2dMsg->header.stamp.toSec()); return; } //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(frameId_, *scan2dMsg, scanOut, tfListener_); + projection.transformLaserScanToPointCloud(scan2dMsg->header.frame_id, *scan2dMsg, scanOut, tfListener_); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -920,18 +978,62 @@ void GuiWrapper::commonStereoCallback( { return; } - Transform t = odomT.inverse() * sensorT; - pclScan = util3d::transformPointCloud(pclScan, t); - + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; } } scan = util3d::laserScan2dFromPointCloud(*pclScan); } else if(scan3dMsg.get() != 0) { - pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); - pcl::fromROSMsg(*scan3dMsg, *pclScan); - scan = util3d::laserScanFromPointCloud(*pclScan); + bool containNormals = false; + for(unsigned int i=0; ifields.size(); ++i) + { + if(scan3dMsg->fields[i].name.compare("normal_x") == 0) + { + containNormals = true; + break; + } + } + + // sync with odometry stamp + scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp); + if(scanLocalTransform.isNull()) + { + ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec()); + return; + } + if(odomHeader.stamp != scan3dMsg->header.stamp) + { + if(!odomT.isNull()) + { + Transform sensorT = getTransform(odomHeader.frame_id, frameId_, scan3dMsg->header.stamp); + if(sensorT.isNull()) + { + ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry " + "stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomHeader.stamp.toSec()); + } + else + { + scanLocalTransform = odomT.inverse() * sensorT * scanLocalTransform; + } + + } + } + + if(containNormals) + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = util3d::laserScanFromPointCloud(*pclScan); + } + else + { + pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); + pcl::fromROSMsg(*scan3dMsg, *pclScan); + + scan = util3d::laserScanFromPointCloud(*pclScan); + } } if(odomInfoMsg.get()) @@ -954,8 +1056,10 @@ void GuiWrapper::commonStereoCallback( rtabmap::OdometryEvent odomEvent( rtabmap::SensorData( scan, - scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, - scan2dMsg.get()?(int)scan2dMsg->range_max:0, + LaserScanInfo( + scan2dMsg.get()?(int)scan2dMsg->ranges.size():0, + scan2dMsg.get()?(int)scan2dMsg->range_max:0, + scanLocalTransform), left, right, stereoModel, diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index 8e7c295d..ce63ff4a 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -99,9 +99,6 @@ public: 0, false, false, - false, - false, - false, nodes_); mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id); diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index 32f2cb6c..3c99afae 100644 --- a/src/MapsManager.cpp +++ b/src/MapsManager.cpp @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -47,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP #include +#include #include #endif #endif @@ -54,96 +56,38 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace rtabmap; MapsManager::MapsManager(bool usePublicNamespace) : - cloudDecimation_(4), - cloudMaxDepth_(4.0), // meters - cloudMinDepth_(0.0), // meters - cloudVoxelSize_(0.05), // meters - cloudFloorCullingHeight_(0.0), - cloudCeilingCullingHeight_(0.0), - cloudOutputVoxelized_(false), - cloudFrustumCulling_(false), - cloudNoiseFilteringRadius_(0.0), - cloudNoiseFilteringMinNeighbors_(5), - scanDecimation_(0), - scanVoxelSize_(0.0), - scanOutputVoxelized_(false), - projMaxGroundAngle_(45.0), // degrees - projMinClusterSize_(20), - projMaxObstaclesHeight_(2.0), // meters (<=0 disabled) - projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true) - projDetectFlatObstacles_(false), - projMapFrame_(false), + cloudOutputVoxelized_(true), gridCellSize_(0.05), // meters + gridIncremental_(false), gridSize_(0), // meters gridEroded_(false), footprintRadius_(0.0), - gridUnknownSpaceFilled_(false), - gridMaxUnknownSpaceFilledRange_(6.0), mapFilterRadius_(0.0), mapFilterAngle_(30.0), // degrees mapCacheCleanup_(true), - negativePosesIgnored_(false), + negativePosesIgnored_(true), + assembledObstacles_(new pcl::PointCloud), + assembledGround_(new pcl::PointCloud), + occupancyGrid_(new OccupancyGrid), octomap_(0), - octomapTreeDepth_(16), - octomapGroundIsObstacle_(false) + octomapTreeDepth_(16) { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - // cloud map stuff - pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_); - pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_); - pnh.param("cloud_min_depth", cloudMinDepth_, cloudMinDepth_); - pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); - pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_); - pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_); - if(cloudFloorCullingHeight_ > 0 && - cloudCeilingCullingHeight_ > 0 && - cloudCeilingCullingHeight_ < cloudFloorCullingHeight_) - { - ROS_WARN("\"cloud_floor_culling_height\" should be lower than \"cloud_ceiling_culling_height\", setting \"cloud_ceiling_culling_height\" to 0 (disabled)."); - cloudCeilingCullingHeight_ = 0; - } - pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_); - pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_); - pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_); - pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_); - - // scan map stuff - pnh.param("scan_decimation", scanDecimation_, scanDecimation_); - pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); - pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_); - - //projection map stuff - pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_); - pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_); - if(pnh.hasParam("proj_max_height") && !pnh.hasParam("proj_max_obstacles_height")) - { - ROS_WARN("Parameter \"proj_max_height\" has been renamed " - "to \"proj_max_obstacles_height\"! Your value is still copied to " - "corresponding parameter."); - pnh.param("proj_max_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_); - } - else - { - pnh.param("proj_max_obstacles_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_); - } - pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_); - pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_); - pnh.param("proj_map_frame", projMapFrame_, projMapFrame_); - // common grid map stuff pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m if(gridCellSize_ <= 0) { ROS_FATAL("\"grid_cell_size\" (%f) should be greater than 0!", gridCellSize_); } + occupancyGrid_->setCellSize(gridCellSize_); + + pnh.param("grid_incremental", gridIncremental_, gridIncremental_); // m pnh.param("grid_size", gridSize_, gridSize_); // m pnh.param("grid_eroded", gridEroded_, gridEroded_); pnh.param("grid_footprint_radius", footprintRadius_, footprintRadius_); - pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_); - pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_); // common map stuff pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); @@ -151,6 +95,17 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_); pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_); + if(pnh.hasParam("scan_output_voxelized")) + { + ROS_WARN("Parameter \"scan_output_voxelized\" has been " + "removed. Use \"cloud_output_voxelized\" instead."); + if(!pnh.hasParam("cloud_output_voxelized")) + { + pnh.getParam("scan_output_voxelized", cloudOutputVoxelized_); + } + } + pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_); + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP octomap_ = new OctoMap(gridCellSize_); @@ -165,7 +120,6 @@ MapsManager::MapsManager(bool usePublicNamespace) : ROS_WARN("octomap_tree_depth cannot be negative, set to 16 instead"); octomapTreeDepth_ = 16; } - pnh.param("octomap_ground_is_obstacle", octomapGroundIsObstacle_, octomapGroundIsObstacle_); #endif #endif @@ -176,43 +130,32 @@ MapsManager::MapsManager(bool usePublicNamespace) : pnh.param("latch", latch, latch); // mapping topics - if(usePublicNamespace) - { - cloudMapPub_ = nh.advertise("cloud_map", 1, latch); - projMapPub_ = nh.advertise("proj_map", 1, latch); - gridMapPub_ = nh.advertise("grid_map", 1, latch); - scanMapPub_ = nh.advertise("scan_map", 1, latch); + ros::NodeHandle nht(usePublicNamespace?"":"~"); + gridMapPub_ = nht.advertise("grid_map", 1, latch); + cloudMapPub_ = nht.advertise("cloud_map", 1, latch); + cloudObstaclesPub_ = nht.advertise("cloud_obstacles", 1, latch); + cloudGroundPub_ = nht.advertise("cloud_ground", 1, latch); + + // deprecated + projMapPub_ = nht.advertise("proj_map", 1, latch); + scanMapPub_ = nht.advertise("scan_map", 1, latch); + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP - octoMapPubBin_ = nh.advertise("octomap_binary", 1, latch); - octoMapPubFull_ = nh.advertise("octomap_full", 1, latch); - octoMapCloud_ = nh.advertise("octomap_cloud", 1, latch); - octoMapEmptySpace_ = nh.advertise("octomap_empty_space", 1, latch); - octoMapProj_ = nh.advertise("octomap_proj", 1, latch); + octoMapPubBin_ = nht.advertise("octomap_binary", 1, latch); + octoMapPubFull_ = nht.advertise("octomap_full", 1, latch); + octoMapCloud_ = nht.advertise("octomap_occupied_space", 1, latch); + octoMapEmptySpace_ = nht.advertise("octomap_empty_space", 1, latch); + octoMapProj_ = nht.advertise("octomap_grid", 1, latch); #endif #endif - } - else - { - cloudMapPub_ = pnh.advertise("cloud_map", 1, latch); - projMapPub_ = pnh.advertise("proj_map", 1, latch); - gridMapPub_ = pnh.advertise("grid_map", 1, latch); - scanMapPub_ = pnh.advertise("scan_map", 1, latch); -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - octoMapPubBin_ = pnh.advertise("octomap_binary", 1, latch); - octoMapPubFull_ = pnh.advertise("octomap_full", 1, latch); - octoMapCloud_ = pnh.advertise("octomap_cloud", 1, latch); - octoMapEmptySpace_ = pnh.advertise("octomap_cloud_ground", 1, latch); - octoMapProj_ = pnh.advertise("octomap_proj", 1, latch); -#endif -#endif - } } MapsManager::~MapsManager() { clear(); + delete occupancyGrid_; + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP if(octomap_) @@ -224,12 +167,106 @@ MapsManager::~MapsManager() { #endif } +void parameterMoved( + ros::NodeHandle & nh, + const std::string & rosName, + const std::string & parameterName, + ParametersMap & parameters) +{ + if(nh.hasParam(rosName)) + { + ParametersMap::const_iterator iter = Parameters::getDefaultParameters().find(parameterName); + if(iter != Parameters::getDefaultParameters().end()) + { + ROS_WARN("Parameter \"%s\" has moved from " + "rtabmap_ros to rtabmap library. Use " + "parameter \"%s\" instead. The value is still " + "copied to new parameter name.", + rosName.c_str(), + parameterName.c_str()); + std::string type = Parameters::getType(parameterName); + if(type.compare("float") || type.compare("double")) + { + double v = uStr2Double(iter->second); + nh.getParam(rosName, v); + parameters.insert(ParametersPair(parameterName, uNumber2Str(v))); + } + else if(type.compare("int") || type.compare("unsigned int")) + { + int v = uStr2Int(iter->second); + nh.getParam(rosName, v); + parameters.insert(ParametersPair(parameterName, uNumber2Str(v))); + } + else + { + ROS_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str()); + } + } + else + { + ROS_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str()); + } + } +} + +void MapsManager::backwardCompatibilityParameters(ParametersMap & parameters) const +{ + ros::NodeHandle pnh("~"); + + // removed + if(pnh.hasParam("cloud_frustum_culling")) + { + ROS_WARN("Parameter \"cloud_frustum_culling\" has been removed. OctoMap topics " + "already do it. You can remove it from your launch file."); + } + + // moved + parameterMoved(pnh, "cloud_decimation", Parameters::kGridDepthDecimation(), parameters); + parameterMoved(pnh, "cloud_max_depth", Parameters::kGridDepthMax(), parameters); + parameterMoved(pnh, "cloud_min_depth", Parameters::kGridDepthMin(), parameters); + parameterMoved(pnh, "cloud_voxel_size", Parameters::kGridCellSize(), parameters); + parameterMoved(pnh, "cloud_floor_culling_height", Parameters::kGridMaxGroundHeight(), parameters); + parameterMoved(pnh, "cloud_ceiling_culling_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "cloud_noise_filtering_radius", Parameters::kGridNoiseFilteringRadius(), parameters); + parameterMoved(pnh, "cloud_noise_filtering_min_neighbors", Parameters::kGridNoiseFilteringMinNeighbors(), parameters); + parameterMoved(pnh, "scan_decimation", Parameters::kGridScanDecimation(), parameters); + parameterMoved(pnh, "scan_voxel_size", Parameters::kGridCellSize(), parameters); + parameterMoved(pnh, "proj_max_ground_angle", Parameters::kGridMaxGroundAngle(), parameters); + parameterMoved(pnh, "proj_min_cluster_size", Parameters::kGridMinClusterSize(), parameters); + parameterMoved(pnh, "proj_max_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "proj_max_obstacles_height", Parameters::kGridMaxObstacleHeight(), parameters); + parameterMoved(pnh, "proj_max_ground_height", Parameters::kGridMaxGroundHeight(), parameters); + + parameterMoved(pnh, "proj_detect_flat_obstacles", Parameters::kGridFlatObstacleDetected(), parameters); + parameterMoved(pnh, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters); + parameterMoved(pnh, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters); + parameterMoved(pnh, "grid_unknown_space_filled_max_range", Parameters::kGridScan2dMaxFilledRange(), parameters); + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGrid3DGroundIsObstacle(), parameters); +#endif +#endif +} + +void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) +{ + parameters_ = parameters; + + // don't use grid cell size from parameters as we use grid_cell_size ros param + uInsert(parameters_, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(gridCellSize_))); + occupancyGrid_->parseParameters(parameters_); +} + void MapsManager::clear() { - clouds_.clear(); - cameraModels_.clear(); - projMaps_.clear(); gridMaps_.clear(); + gridMapsViewpoints_.clear(); + assembledGround_->clear(); + assembledObstacles_->clear(); + assembledGroundPoses_.clear(); + assembledObstaclePoses_.clear(); + occupancyGrid_->clear(); #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP octomap_->clear(); @@ -240,6 +277,8 @@ void MapsManager::clear() bool MapsManager::hasSubscribers() const { return cloudMapPub_.getNumSubscribers() != 0 || + cloudObstaclesPub_.getNumSubscribers() != 0 || + cloudGroundPub_.getNumSubscribers() != 0 || projMapPub_.getNumSubscribers() != 0 || gridMapPub_.getNumSubscribers() != 0 || scanMapPub_.getNumSubscribers() != 0 || @@ -264,20 +303,14 @@ std::map MapsManager::getFilteredPoses(const std::map MapsManager::updateMapCaches( const std::map & poses, const rtabmap::Memory * memory, - bool updateCloud, - bool updateProj, bool updateGrid, - bool updateScan, bool updateOctomap, const std::map & signatures) { - if(!updateCloud && !updateProj && !updateGrid && !updateScan && !updateOctomap) + if(!updateGrid && !updateOctomap) { // all false, udpate only those where we have subscribers - updateCloud = cloudMapPub_.getNumSubscribers() != 0; - updateProj = projMapPub_.getNumSubscribers() != 0; - updateGrid = gridMapPub_.getNumSubscribers() != 0; - updateScan = scanMapPub_.getNumSubscribers() != 0; + updateGrid = this->hasSubscribers(); updateOctomap = octoMapPubBin_.getNumSubscribers() != 0 || octoMapPubFull_.getNumSubscribers() != 0 || @@ -305,7 +338,7 @@ std::map MapsManager::updateMapCaches( std::map filteredPoses; // update cache - if(updateCloud || updateProj || updateGrid || updateScan || updateOctomap) + if(updateGrid || updateOctomap) { // filter nodes if(mapFilterRadius_ > 0.0) @@ -349,29 +382,14 @@ std::map MapsManager::updateMapCaches( bool longUpdate = false; if(filteredPoses.size() > 20) { - if(updateCloud && clouds_.size() < 5) + if(updateGrid && gridMaps_.size() < 5) { - ROS_WARN("Many clouds should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-clouds_.size())); - longUpdate = true; - } - else if(updateProj && projMaps_.size() < 5) - { - ROS_WARN("Many occupancy grid map from projections should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-projMaps_.size())); - longUpdate = true; - } - else if(updateGrid && gridMaps_.size() < 5) - { - ROS_WARN("Many occupancy grid map from laser scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size())); - longUpdate = true; - } - else if(updateScan && scans_.size() < 5) - { - ROS_WARN("Many scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-scans_.size())); + ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); longUpdate = true; } #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP - else if(updateOctomap && octomap_->addedNodes().size() < 5) + if(updateOctomap && octomap_->addedNodes().size() < 5) { ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); longUpdate = true; @@ -385,27 +403,7 @@ std::map MapsManager::updateMapCaches( if(!iter->second.isNull()) { rtabmap::SensorData data; - bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first)); - bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first)); - bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first)); - bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first)); - -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - if(!rgbDepthRequired) - { - rgbDepthRequired = updateOctomap && - (iter->first < 0 || - octomap_->addedNodes().empty() || - iter->first > octomap_->addedNodes().rbegin()->first); - } -#endif -#endif - - if(rgbDepthRequired || - depthRequired || - scanRequired || - gridRequired) + if((updateGrid || updateOctomap) && (iter->first < 0 || !uContains(gridMaps_, iter->first))) { UDEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); @@ -415,277 +413,84 @@ std::map MapsManager::updateMapCaches( } else if(memory) { - data = memory->getSignatureDataConst(iter->first); + data = memory->getSignatureDataConst(iter->first, false, false, false, true); } - } - if(data.id() != 0) - { - if(!(data.imageCompressed().empty() && data.imageRaw().empty()) && - !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) && - (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())) + if(data.id() != 0) { - // Which data should we decompress? - cv::Mat image, depth, scan; + cv::Mat ground, obstacles; data.uncompressData( - (rgbDepthRequired||data.stereoCameraModel().isValidForProjection()) ? &image:0, - (rgbDepthRequired||depthRequired) ? &depth:0, - scanRequired||gridRequired?&scan:0); + 0, + 0, + 0, + 0, + &ground, + &obstacles); - pcl::PointCloud::Ptr cloudRGB; - pcl::PointCloud::Ptr cloudXYZ; - if(rgbDepthRequired) + UDEBUG("Adding grid map %d to cache...", iter->first); + + cv::Point3f viewPoint; + if(iter->first > 0 || data.gridCellSize()) { - UDEBUG("rgbDepthRequired"); - if(!image.empty() && !depth.empty()) + viewPoint = data.gridViewPoint(); + gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles))); + gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint)); + } + else + { + // generate tmp occupancy grid for negative ids + // we need the signature + std::map::const_iterator findIter = signatures.find(iter->first); + if(findIter != signatures.end()) { - pcl::IndicesPtr validIndices(new std::vector); - cloudRGB = util3d::cloudRGBFromSensorData( - data, - cloudDecimation_, - cloudMaxDepth_, - cloudMinDepth_, - validIndices.get()); - if(cloudVoxelSize_) - { - cloudRGB = util3d::voxelize(cloudRGB, validIndices, cloudVoxelSize_); - } - if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) - { - pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::copyPointCloud(*cloudRGB, *indices, *tmp); - cloudRGB = tmp; - } + // normally data should be already uncompressed for negative ids + occupancyGrid_->createLocalMap(findIter->second, ground, obstacles, viewPoint); + gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles))); + gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint)); } else { - ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first); + ROS_WARN("%d signature not found in cache?!?!?", iter->first); } } - else if(depthRequired) + if(ground.cols || obstacles.cols) { - UDEBUG("depthRequired"); - if( !depth.empty()) - { - pcl::IndicesPtr validIndices(new std::vector); - cloudXYZ = util3d::cloudFromSensorData( - data, - cloudDecimation_, - cloudMaxDepth_, - cloudMinDepth_, - validIndices.get()); // use gridCellSize since this cloud is only for the projection map - UASSERT(gridCellSize_ > 0); - cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_); - if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0) - { - pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_); - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::copyPointCloud(*cloudXYZ, *indices, *tmp); - cloudXYZ = tmp; - } - } - else - { - ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first); - } - } - - if(cloudRGB.get()) - { - uInsert(clouds_, std::make_pair(iter->first, cloudRGB)); - - // Make sure that image size is set in camera models. - // The camera models are used when cloud_frustum_culling=true. - std::vector models; - if(data.stereoCameraModel().isValidForProjection()) - { - //insert only the left camera model - rtabmap::CameraModel model = data.stereoCameraModel().left(); - model.setImageSize(cv::Size(data.imageRaw().cols, data.imageRaw().rows)); - models.push_back(model); - } - else if(data.cameraModels().size()) - { - UASSERT_MSG(data.imageRaw().cols % data.cameraModels().size() == 0, - uFormat("data.imageRaw().cols=%d data.cameraModels().size()=%d", - data.imageRaw().cols, (int)data.cameraModels().size()).c_str()); - - models.resize(data.cameraModels().size()); - for(unsigned int i=0; ifirst, models)); - } - - if(depthRequired || updateOctomap) - { - UDEBUG("Creating proj map / octomap for %d...", iter->first); - cv::Mat ground, obstacles; - if(cloudRGB.get()) - { - pcl::PointCloud::Ptr cloudClipped = cloudRGB; - if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) - { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); - } - if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_) - { - cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_); - } - if(cloudClipped->size()) - { - // add pose rotation without yaw - float roll, pitch, yaw; - iter->second.getEulerAngles(roll, pitch, yaw); - cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0)); - - pcl::IndicesPtr groundIndices, obstaclesIndices; - util3d::segmentObstaclesFromGround( - cloudClipped, - groundIndices, - obstaclesIndices, - 20, - projMaxGroundAngle_*M_PI/180.0, - gridCellSize_*2.0f, - projMinClusterSize_, - projDetectFlatObstacles_, - projMaxGroundHeight_); - - pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); - pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); - - if(groundIndices->size()) - { - pcl::copyPointCloud(*cloudClipped, *groundIndices, *groundCloud); - } - - if(obstaclesIndices->size()) - { - pcl::copyPointCloud(*cloudClipped, *obstaclesIndices, *obstaclesCloud); - } - - if(updateProj) - { - util3d::occupancy2DFromGroundObstacles( - groundCloud, - obstaclesCloud, - ground, - obstacles, - gridCellSize_); - uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); - } - -#ifdef WITH_OCTOMAP_ROS -#ifdef RTABMAP_OCTOMAP - if(updateOctomap) - { - Transform tinv = Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0).inverse(); - groundCloud = util3d::transformPointCloud(groundCloud, tinv); - obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv); - if(octomapGroundIsObstacle_) - { - *obstaclesCloud += *groundCloud; - groundCloud->clear(); - } - octomap_->addToCache(iter->first, groundCloud, obstaclesCloud); - } -#endif -#endif - } - } - else if(updateProj && cloudXYZ.get()) - { - pcl::PointCloud::Ptr cloudClipped = cloudXYZ; - if(cloudClipped->size() && projMaxObstaclesHeight_ > 0) - { - cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), projMaxObstaclesHeight_); - } - if(cloudClipped->size()) - { - // add pose rotation without yaw - float roll, pitch, yaw; - iter->second.getEulerAngles(roll, pitch, yaw); - cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0)); - - pcl::IndicesPtr groundIndices, obstaclesIndices; - util3d::segmentObstaclesFromGround( - cloudClipped, - groundIndices, - obstaclesIndices, - 20, - projMaxGroundAngle_*M_PI/180.0, - gridCellSize_*2.0f, - projMinClusterSize_, - projDetectFlatObstacles_, - projMaxGroundHeight_); - - util3d::occupancy2DFromGroundObstacles( - cloudClipped, - groundIndices, - obstaclesIndices, - ground, - obstacles, - gridCellSize_); - uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); - } - } - } - - if(scanRequired || gridRequired) - { - if(scan.cols && (scanRequired || scanVoxelSize_ > 0.0 || scanDecimation_ > 1)) - { - if(scanDecimation_ > 1) - { - scan = util3d::downsample(scan, scanDecimation_); - } - - if(scanRequired || scanVoxelSize_ > 0.0) - { - pcl::PointCloud::Ptr scanCloud = util3d::laserScanToPointCloud(scan); - if(scanVoxelSize_ > 0.0) - { - scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_); - if(gridRequired && scan.type() == CV_32FC2) - { - scan = util3d::laserScan2dFromPointCloud(*scanCloud); - } - } - - if(scanRequired) - { - uInsert(scans_, std::make_pair(iter->first, scanCloud)); - } - } - } - - if(gridRequired && scan.type() == CV_32FC2) - { - cv::Mat ground, obstacles; - util3d::occupancy2DFromLaserScan( - scan, - ground, - obstacles, - gridCellSize_, - data.id() < 0 || gridUnknownSpaceFilled_, - data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange()); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); - } + occupancyGrid_->addToCache(iter->first, ground, obstacles); } } else { - ROS_ERROR("Some data missing for node %d to update the maps (image=%d, depth=%d, camera=%d)", - iter->first, - !(data.imageCompressed().empty() && data.imageRaw().empty())?1:0, - !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty())?1:0, - (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())?1:0); + ROS_ERROR("Data missing for node %d to update the maps", iter->first); } } + +#ifdef WITH_OCTOMAP_ROS +#ifdef RTABMAP_OCTOMAP + if(updateOctomap && + (iter->first < 0 || + octomap_->addedNodes().empty() || + iter->first > octomap_->addedNodes().rbegin()->first)) + { + std::map >::iterator mter = gridMaps_.find(iter->first); + std::map::iterator pter = gridMapsViewpoints_.find(iter->first); + if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end()) + { + if((mter->second.first.empty() || mter->second.first.channels() > 2) && + (mter->second.second.empty() || mter->second.second.channels() > 2)) + { + octomap_->addToCache(iter->first, mter->second.first, mter->second.second, pter->second); + } + else if(!mter->second.first.empty() && !mter->second.second.empty()) + { + ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + iter->first); + } + } + } +#endif +#endif } else { @@ -703,50 +508,12 @@ std::map MapsManager::updateMapCaches( } #endif #endif - - // cleanup not used nodes - UDEBUG("Cleanup not used nodes"); - for(std::map::Ptr >::iterator iter=clouds_.begin(); - iter!=clouds_.end();) - { - if(!uContains(poses, iter->first)) - { - clouds_.erase(iter++); - } - else - { - ++iter; - } - } - for(std::map::Ptr >::iterator iter=scans_.begin(); - iter!=scans_.end();) - { - if(!uContains(poses, iter->first)) - { - scans_.erase(iter++); - } - else - { - ++iter; - } - } - for(std::map >::iterator iter=projMaps_.begin(); - iter!=projMaps_.end();) - { - if(!uContains(poses, iter->first)) - { - projMaps_.erase(iter++); - } - else - { - ++iter; - } - } for(std::map >::iterator iter=gridMaps_.begin(); iter!=gridMaps_.end();) { if(!uContains(poses, iter->first)) { + UASSERT(gridMapsViewpoints_.erase(iter->first) != 0); gridMaps_.erase(iter++); } else @@ -754,18 +521,6 @@ std::map MapsManager::updateMapCaches( ++iter; } } - for(std::map >::iterator iter=cameraModels_.begin(); - iter!=cameraModels_.end();) - { - if(!uContains(poses, iter->first)) - { - cameraModels_.erase(iter++); - } - else - { - ++iter; - } - } if(longUpdate) { @@ -784,108 +539,168 @@ void MapsManager::publishMaps( UDEBUG("Publishing maps..."); // publish maps - if(cloudMapPub_.getNumSubscribers()) + if(cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudObstaclesPub_.getNumSubscribers() || + cloudGroundPub_.getNumSubscribers()) { // generate the assembled cloud! UTimer time; - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - int count = 0; - std::list > negativePoses; + + if(scanMapPub_.getNumSubscribers()) + { + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) + { + ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); + } + else + { + ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic instead."); + } + } + + // detect if the graph has changed, if so, recreate the clouds + bool graphGroundChanged = false; + bool graphObstacleChanged = false; + bool updateGround = cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudGroundPub_.getNumSubscribers(); + bool updateObstacles = cloudMapPub_.getNumSubscribers() || + scanMapPub_.getNumSubscribers() || + cloudObstaclesPub_.getNumSubscribers(); + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + std::map::const_iterator jter; + if(updateGround) + { + jter = assembledGroundPoses_.find(iter->first); + if(jter != assembledGroundPoses_.end()) + { + UASSERT(!iter->second.isNull() && !jter->second.isNull()); + if(iter->second.getDistanceSquared(jter->second) > 0.0001) + { + graphGroundChanged = true; + } + } + } + if(updateObstacles) + { + jter = assembledObstaclePoses_.find(iter->first); + if(jter != assembledObstaclePoses_.end()) + { + UASSERT(!iter->second.isNull() && !jter->second.isNull()); + if(iter->second.getDistanceSquared(jter->second) > 0.0001) + { + graphObstacleChanged = true; + } + } + } + } + int countObstacles = 0; + int countGrounds = 0; + if(graphGroundChanged) + { + assembledGround_->clear(); + assembledGroundPoses_.clear(); + } + if(graphObstacleChanged) + { + assembledObstacles_->clear(); + assembledObstaclePoses_.clear(); + } for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { if(iter->first > 0) { - std::map::Ptr >::iterator jter = clouds_.find(iter->first); - if(jter != clouds_.end()) + std::map >::iterator jter = gridMaps_.find(iter->first); + if(updateGround && + (graphGroundChanged || assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end())) { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - ++count; + assembledGroundPoses_.insert(*iter); + if(jter!=gridMaps_.end() && jter->second.first.cols) + { + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first, iter->second); + *assembledGround_+=*transformed; + ++countGrounds; + } + } + if(updateObstacles && + (graphObstacleChanged || assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end())) + { + assembledObstaclePoses_.insert(*iter); + if(jter!=gridMaps_.end() && jter->second.second.cols) + { + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.second, iter->second); + *assembledObstacles_+=*transformed; + ++countObstacles; + } } } - else + } + + if(cloudOutputVoxelized_) + { + UASSERT(gridCellSize_ > 0.0); + if(countGrounds && assembledGround_->size()) { - negativePoses.push_back(*iter); + assembledGround_ = util3d::voxelize(assembledGround_, gridCellSize_); } - } - - for(std::list >::reverse_iterator iter=negativePoses.rbegin(); iter!=negativePoses.rend(); ++iter) - { - std::map::Ptr >::iterator jter = clouds_.find(iter->first); - - if(jter != clouds_.end() && jter->second->size()) + if(countObstacles && assembledObstacles_->size()) { - std::map >::iterator kter = cameraModels_.find(iter->first); - if(cloudFrustumCulling_ && kter != cameraModels_.end() && assembledCloud->size()) - { - for(unsigned int i=0; isecond.size(); ++i) - { - if(kter->second[i].isValidForProjection()) - { - assembledCloud = util3d::frustumFiltering( - assembledCloud, - iter->second, // FIXME: should include camera local transform - kter->second[i].horizontalFOV(), - kter->second[i].verticalFOV(), - 0.0f, - cloudMaxDepth_>0.0?cloudMaxDepth_:999999., - true); - //ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size()); - - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - ++count; - } - } - } - else - { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - if(assembledCloud->size()) - { - *assembledCloud+=*transformed; - } - else - { - assembledCloud = transformed; - } - ++count; - } + assembledObstacles_ = util3d::voxelize(assembledObstacles_, gridCellSize_); } } - if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0)) - { - assembledCloud = util3d::passThrough(assembledCloud, "z", - cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0, - cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0); - } + ROS_INFO("Assembled %d obstacle and %d ground clouds (%d points, %fs)", + countObstacles, countGrounds, (int)(assembledGround_->size() + assembledObstacles_->size()), time.ticks()); - if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_) - { - assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_); - } - - ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks()); - - if(assembledCloud->size()) + if(cloudGroundPub_.getNumSubscribers()) { sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); + pcl::toROSMsg(*assembledGround_, *cloudMsg); cloudMsg->header.stamp = stamp; cloudMsg->header.frame_id = mapFrameId; - cloudMapPub_.publish(cloudMsg); + cloudGroundPub_.publish(cloudMsg); } - else if(poses.size() - negativePoses.size()) + if(cloudObstaclesPub_.getNumSubscribers()) { - ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size()); + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(*assembledObstacles_, *cloudMsg); + cloudMsg->header.stamp = stamp; + cloudMsg->header.frame_id = mapFrameId; + cloudObstaclesPub_.publish(cloudMsg); + } + if(cloudMapPub_.getNumSubscribers() || scanMapPub_.getNumSubscribers()) + { + pcl::PointCloud cloud = *assembledObstacles_ + *assembledGround_; + sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); + pcl::toROSMsg(cloud, *cloudMsg); + cloudMsg->header.stamp = stamp; + cloudMsg->header.frame_id = mapFrameId; + + if(cloudMapPub_.getNumSubscribers()) + { + cloudMapPub_.publish(cloudMsg); + } + if(scanMapPub_.getNumSubscribers()) + { + scanMapPub_.publish(cloudMsg); + } } } else if(mapCacheCleanup_) { - clouds_.clear(); - cameraModels_.clear(); + assembledGround_->clear(); + assembledObstacles_->clear(); + assembledGroundPoses_.clear(); + assembledObstaclePoses_.clear(); } + #ifdef WITH_OCTOMAP_ROS #ifdef RTABMAP_OCTOMAP if(octoMapPubBin_.getNumSubscribers() || @@ -913,14 +728,14 @@ void MapsManager::publishMaps( if(octoMapCloud_.getNumSubscribers() || octoMapEmptySpace_.getNumSubscribers()) { sensor_msgs::PointCloud2 msg; - pcl::IndicesPtr obstacles(new std::vector); - pcl::IndicesPtr ground(new std::vector); - pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacles.get(), ground.get()); + pcl::IndicesPtr obstacleIndices(new std::vector); + pcl::IndicesPtr emptyIndices(new std::vector); + pcl::PointCloud::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get()); if(octoMapCloud_.getNumSubscribers()) { pcl::PointCloud cloudObstacles; - pcl::copyPointCloud(*cloud, *obstacles, cloudObstacles); + pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles); pcl::toROSMsg(cloudObstacles, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; @@ -928,9 +743,9 @@ void MapsManager::publishMaps( } if(octoMapEmptySpace_.getNumSubscribers()) { - pcl::PointCloud cloudGround; - pcl::copyPointCloud(*cloud, *ground, cloudGround); - pcl::toROSMsg(cloudGround, msg); + pcl::PointCloud cloudEmptySpace; + pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace); + pcl::toROSMsg(cloudEmptySpace, msg); msg.header.frame_id = mapFrameId; msg.header.stamp = stamp; octoMapEmptySpace_.publish(msg); @@ -970,109 +785,40 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size()); + ROS_WARN("Octomap projection map is empty! (poses=%d octomap nodes=%d). " + "Make sure you activated \"%s\" and \"%s\" to true. " + "See \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" for more info.", + (int)poses.size(), (int)octomap_->octree()->size(), + Parameters::kGrid3D().c_str(), Parameters::kGridFromDepth().c_str()); } } } - else + else if(mapCacheCleanup_) { octomap_->clear(); } #endif #endif - - if(scanMapPub_.getNumSubscribers()) + if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers()) { - // generate the assembled scan cloud! - UTimer time; - pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - int count = 0; - std::list > negativePoses; - for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + if(projMapPub_.getNumSubscribers()) { - if(iter->first > 0) + if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() && + !uStr2Bool(parameters_.at(Parameters::kGridFromDepth()))) { - std::map::Ptr >::iterator jter = scans_.find(iter->first); - if(jter != scans_.end() && jter->second->size()) - { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); - *assembledCloud+=*transformed; - ++count; - } + ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic " + "instead with . " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + Parameters::kGridFromDepth().c_str()); } - // negative poses are not used - } - - if(assembledCloud->size()) - { - if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_) + else { - assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_); + ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic instead."); } - - ROS_INFO("Assembled %d scans (%fs)", count, time.ticks()); - - sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); - pcl::toROSMsg(*assembledCloud, *cloudMsg); - cloudMsg->header.stamp = stamp; - cloudMsg->header.frame_id = mapFrameId; - scanMapPub_.publish(cloudMsg); } - else if(poses.size()) - { - ROS_WARN("Scan map is empty! (poses=%d, scans=%d)", (int)poses.size(), (int)scans_.size()); - } - } - else if(mapCacheCleanup_) - { - scans_.clear(); - } - if(projMapPub_.getNumSubscribers()) - { - // create the projection map - float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; - cv::Mat pixels = this->generateProjMap(poses, xMin, yMin, gridCellSize); - - if(!pixels.empty()) - { - //init - nav_msgs::OccupancyGrid map; - map.info.resolution = gridCellSize; - map.info.origin.position.x = 0.0; - map.info.origin.position.y = 0.0; - map.info.origin.position.z = 0.0; - map.info.origin.orientation.x = 0.0; - map.info.origin.orientation.y = 0.0; - map.info.origin.orientation.z = 0.0; - map.info.origin.orientation.w = 1.0; - - map.info.width = pixels.cols; - map.info.height = pixels.rows; - map.info.origin.position.x = xMin; - map.info.origin.position.y = yMin; - map.data.resize(map.info.width * map.info.height); - - memcpy(map.data.data(), pixels.data, map.info.width * map.info.height); - - map.header.frame_id = mapFrameId; - map.header.stamp = stamp; - - projMapPub_.publish(map); - } - else if(poses.size()) - { - ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size()); - } - } - else if(mapCacheCleanup_) - { - projMaps_.clear(); - } - - if(gridMapPub_.getNumSubscribers()) - { // create the grid map float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f; cv::Mat pixels = this->generateGridMap(poses, xMin, yMin, gridCellSize); @@ -1101,36 +847,28 @@ void MapsManager::publishMaps( map.header.frame_id = mapFrameId; map.header.stamp = stamp; - gridMapPub_.publish(map); + if(gridMapPub_.getNumSubscribers()) + { + gridMapPub_.publish(map); + } + if(projMapPub_.getNumSubscribers()) + { + projMapPub_.publish(map); + } } else if(poses.size()) { ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size()); } } - else if(mapCacheCleanup_) + + if(!this->hasSubscribers() && mapCacheCleanup_) { gridMaps_.clear(); + gridMapsViewpoints_.clear(); } } -cv::Mat MapsManager::generateProjMap( - const std::map & poses, - float & xMin, - float & yMin, - float & gridCellSize) -{ - gridCellSize = gridCellSize_; - return util3d::create2DMapFromOccupancyLocalMaps( - poses, - projMaps_, - gridCellSize_, - xMin, yMin, - gridSize_, - gridEroded_, - footprintRadius_); -} - cv::Mat MapsManager::generateGridMap( const std::map & poses, float & xMin, @@ -1138,14 +876,23 @@ cv::Mat MapsManager::generateGridMap( float & gridCellSize) { gridCellSize = gridCellSize_; - cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps( - poses, - gridMaps_, - gridCellSize_, - xMin, yMin, - gridSize_, - gridEroded_, - footprintRadius_); + cv::Mat map; + if(gridIncremental_) + { + occupancyGrid_->update(poses, gridSize_, footprintRadius_); + map = occupancyGrid_->getMap(xMin, yMin); + } + else + { + map = util3d::create2DMapFromOccupancyLocalMaps( + poses, + gridMaps_, + gridCellSize_, + xMin, yMin, + gridSize_, + gridEroded_, + footprintRadius_); + } return map; } diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 9d8f8431..fba69cc9 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -332,16 +332,42 @@ rtabmap::CameraModel cameraModelFromROS( const sensor_msgs::CameraInfo & camInfo, const rtabmap::Transform & localTransform) { - image_geometry::PinholeCameraModel model; - model.fromCameraInfo(camInfo); + cv::Mat D; + if(camInfo.D.size()) + { + D = cv::Mat(1, camInfo.D.size(), CV_64FC1); + memcpy(D.data, camInfo.D.data(), D.cols*sizeof(double)); + } + + cv:: Mat K; + UASSERT(camInfo.K.empty() || camInfo.K.size() == 9); + if(!camInfo.K.empty()) + { + K = cv::Mat(3, 3, CV_64FC1); + memcpy(K.data, camInfo.K.elems, 9*sizeof(double)); + } + + cv:: Mat R; + UASSERT(camInfo.R.empty() || camInfo.R.size() == 9); + if(!camInfo.R.empty()) + { + R = cv::Mat(3, 3, CV_64FC1); + memcpy(R.data, camInfo.R.elems, 9*sizeof(double)); + } + + cv:: Mat P; + UASSERT(camInfo.P.empty() || camInfo.P.size() == 12); + if(!camInfo.P.empty()) + { + P = cv::Mat(3, 4, CV_64FC1); + memcpy(P.data, camInfo.P.elems, 12*sizeof(double)); + } + return rtabmap::CameraModel( - model.fx(), - model.fy(), - model.cx(), - model.cy(), - localTransform, - 0.0, - cv::Size(model.fullResolution().width, model.fullResolution().height)); + "ros", + cv::Size(camInfo.width, camInfo.height), + K, D, R, P, + localTransform); } void cameraModelToROS( const rtabmap::CameraModel & model, @@ -401,16 +427,11 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS( const sensor_msgs::CameraInfo & rightCamInfo, const rtabmap::Transform & localTransform) { - image_geometry::StereoCameraModel model; - model.fromCameraInfo(leftCamInfo, rightCamInfo); return rtabmap::StereoCameraModel( - model.left().fx(), - model.left().fy(), - model.left().cx(), - model.left().cy(), - model.baseline(), - localTransform, - cv::Size(model.left().fullResolution().width, model.left().fullResolution().height)); + "ros", + cameraModelFromROS(leftCamInfo, localTransform), + cameraModelFromROS(rightCamInfo, localTransform), + rtabmap::Transform()); } void mapDataFromROS( @@ -587,8 +608,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) stereoModel.isValidForProjection()? rtabmap::SensorData( compressedMatFromBytes(msg.laserScan), - msg.laserScanMaxPts, - msg.laserScanMaxRange, + rtabmap::LaserScanInfo( + msg.laserScanMaxPts, + msg.laserScanMaxRange, + transformFromGeometryMsg(msg.laserScanLocalTransform)), compressedMatFromBytes(msg.image), compressedMatFromBytes(msg.depth), stereoModel, @@ -597,8 +620,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) compressedMatFromBytes(msg.userData)): rtabmap::SensorData( compressedMatFromBytes(msg.laserScan), - msg.laserScanMaxPts, - msg.laserScanMaxRange, + rtabmap::LaserScanInfo( + msg.laserScanMaxPts, + msg.laserScanMaxRange, + transformFromGeometryMsg(msg.laserScanLocalTransform)), compressedMatFromBytes(msg.image), compressedMatFromBytes(msg.depth), models, @@ -608,6 +633,11 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) s.setWords(words); s.setWords3(words3D); s.setWordsDescriptors(wordsDescriptors); + s.sensorData().setOccupancyGrid( + compressedMatFromBytes(msg.grid_ground), + compressedMatFromBytes(msg.grid_obstacles), + msg.grid_cell_size, + point3fFromROS(msg.grid_view_point)); return s; } void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg) @@ -624,8 +654,13 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth); compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan); compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData); - msg.laserScanMaxPts = signature.sensorData().laserScanMaxPts(); - msg.laserScanMaxRange = signature.sensorData().laserScanMaxRange(); + compressedMatToBytes(signature.sensorData().gridGroundCellsCompressed(), msg.grid_ground); + compressedMatToBytes(signature.sensorData().gridObstacleCellsCompressed(), msg.grid_obstacles); + point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point); + msg.grid_cell_size = signature.sensorData().gridCellSize(); + msg.laserScanMaxPts = signature.sensorData().laserScanInfo().maxPoints(); + msg.laserScanMaxRange = signature.sensorData().laserScanInfo().maxRange(); + transformToGeometryMsg(signature.sensorData().laserScanInfo().localTransform(), msg.laserScanLocalTransform); msg.baseline = 0; if(signature.sensorData().cameraModels().size()) { diff --git a/src/nodelets/icp_odometry.cpp b/src/nodelets/icp_odometry.cpp index 72aeee4b..6c96fcb2 100644 --- a/src/nodelets/icp_odometry.cpp +++ b/src/nodelets/icp_odometry.cpp @@ -93,9 +93,10 @@ private: void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg) { // make sure the frame of the laser is updated too - if(getTransform(this->frameId(), + Transform localScanTransform = getTransform(this->frameId(), scanMsg->header.frame_id, - scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull()) + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); + if(localScanTransform.isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); return; @@ -104,7 +105,7 @@ private: //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(this->frameId(), *scanMsg, scanOut, this->tfListener()); + projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -112,8 +113,7 @@ private: rtabmap::SensorData data( scan, - (int)scanMsg->ranges.size(), - scanMsg->range_max, + LaserScanInfo((int)scanMsg->ranges.size(), scanMsg->range_max, localScanTransform), cv::Mat(), cv::Mat(), CameraModel(), @@ -147,10 +147,6 @@ private: { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!localScanTransform.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, localScanTransform); - } scan = util3d::laserScanFromPointCloud(*pclScan); } else @@ -158,11 +154,6 @@ private: pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!localScanTransform.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, localScanTransform); - } - if(scanCloudNormalK_ > 0) { //compute normals @@ -179,8 +170,7 @@ private: rtabmap::SensorData data( scan, - scanCloudMaxPoints_, - 0, + LaserScanInfo(scanCloudMaxPoints_, 0, localScanTransform), cv::Mat(), cv::Mat(), CameraModel(), diff --git a/src/nodelets/rgbdicp_odometry.cpp b/src/nodelets/rgbdicp_odometry.cpp index 3431b8d6..80f0de44 100644 --- a/src/nodelets/rgbdicp_odometry.cpp +++ b/src/nodelets/rgbdicp_odometry.cpp @@ -266,12 +266,14 @@ private: cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); cv::Mat scan; + Transform localScanTransform = Transform::getIdentity(); if(scanMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(this->frameId(), + localScanTransform = getTransform(this->frameId(), scanMsg->header.frame_id, - scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull()) + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)); + if(localScanTransform.isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec()); return; @@ -280,7 +282,7 @@ private: //transform in frameId_ frame sensor_msgs::PointCloud2 scanOut; laser_geometry::LaserProjection projection; - projection.transformLaserScanToPointCloud(this->frameId(), *scanMsg, scanOut, this->tfListener()); + projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener()); pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(scanOut, *pclScan); @@ -297,7 +299,7 @@ private: break; } } - Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); + localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp); if(localScanTransform.isNull()) { ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec()); @@ -308,10 +310,6 @@ private: { pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!localScanTransform.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, localScanTransform); - } scan = util3d::laserScanFromPointCloud(*pclScan); } else @@ -319,11 +317,6 @@ private: pcl::PointCloud::Ptr pclScan(new pcl::PointCloud); pcl::fromROSMsg(*cloudMsg, *pclScan); - if(!localScanTransform.isIdentity()) - { - pclScan = util3d::transformPointCloud(pclScan, localScanTransform); - } - if(scanCloudNormalK_ > 0) { //compute normals @@ -341,8 +334,10 @@ private: rtabmap::SensorData data( scan, - scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0, - scanMsg.get() != 0?scanMsg->range_max:0, + LaserScanInfo( + scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0, + scanMsg.get() != 0?scanMsg->range_max:0, + localScanTransform), ptrImage->image, ptrDepth->image, rtabmapModel,