From 0abb9aad43fc5b665ad533102089f607e5a82759 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 20 Oct 2014 21:48:59 +0000 Subject: [PATCH] ros-pkg: added occupancy 2d grid generation from 3d cloud projection in map_assembler node (default not activated). Added also node filtering by default in map_assembler node. git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1905 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- launch/tests/test_stereo_mapping_bag.launch | 1 + src/GridMapAssemblerNode.cpp | 2 +- src/MapAssemblerNode.cpp | 120 +++++++++++++++++--- 3 files changed, 109 insertions(+), 14 deletions(-) diff --git a/launch/tests/test_stereo_mapping_bag.launch b/launch/tests/test_stereo_mapping_bag.launch index 83364a9d..0fb7ea40 100644 --- a/launch/tests/test_stereo_mapping_bag.launch +++ b/launch/tests/test_stereo_mapping_bag.launch @@ -108,6 +108,7 @@ + diff --git a/src/GridMapAssemblerNode.cpp b/src/GridMapAssemblerNode.cpp index 11275330..6aa4b798 100644 --- a/src/GridMapAssemblerNode.cpp +++ b/src/GridMapAssemblerNode.cpp @@ -46,7 +46,7 @@ public: gridCellSize_(0.05), gridUnknownSpaceFilled_(true), filterRadius_(0.5), - filterAngle_(30.0) + filterAngle_(30.0) // degrees { ros::NodeHandle pnh("~"); pnh.param("cell_size", gridCellSize_, gridCellSize_); diff --git a/src/MapAssemblerNode.cpp b/src/MapAssemblerNode.cpp index be971f29..4fab95d8 100644 --- a/src/MapAssemblerNode.cpp +++ b/src/MapAssemblerNode.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include using namespace rtabmap; @@ -44,7 +45,15 @@ public: cloudDecimation_(4), cloudMaxDepth_(4.0), cloudVoxelSize_(0.02), - scanVoxelSize_(0.01) + scanVoxelSize_(0.01), + nodeFilteringAngle_(30), // degrees + nodeFilteringRadius_(0.5), + computeOccupancyGrid_(false), + gridCellSize_(0.05), + groundMaxAngle_(M_PI_4), + clusterMinSize_(20), + emptyCellFillingRadius_(1), + maxHeight_(0) { ros::NodeHandle pnh("~"); pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_); @@ -52,11 +61,29 @@ public: pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_); pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); + pnh.param("filter_radius", nodeFilteringRadius_, nodeFilteringRadius_); + pnh.param("filter_angle", nodeFilteringAngle_, nodeFilteringAngle_); + + pnh.param("occupancy_grid", computeOccupancyGrid_, computeOccupancyGrid_); + pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_); + pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_); + pnh.param("occupancy_cluster_min_size", clusterMinSize_, clusterMinSize_); + pnh.param("occupancy_empty_filling_radius", emptyCellFillingRadius_, emptyCellFillingRadius_); + pnh.param("occupancy_max_height", maxHeight_, maxHeight_); + + UASSERT(gridCellSize_ > 0); + UASSERT(emptyCellFillingRadius_ >= 0); + UASSERT(maxHeight_ >= 0); + ros::NodeHandle nh; mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this); assembledMapClouds_ = nh.advertise("assembled_clouds", 1); assembledMapScans_ = nh.advertise("assembled_scans", 1); + if(computeOccupancyGrid_) + { + occupancyMapPub_ = nh.advertise("grid_projection_map", 1); + } } ~MapAssembler() @@ -152,6 +179,20 @@ public: cloud = util3d::transformPointCloud(cloud, localTransform); rgbClouds_.insert(std::make_pair(id, cloud)); + + if(computeOccupancyGrid_) + { + pcl::PointCloud::Ptr cloudClipped = cloud; + if(maxHeight_ > 0) + { + cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits::min(), maxHeight_); + } + cv::Mat ground, obstacles; + if(util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_)) + { + occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles))); + } + } } } } @@ -176,20 +217,28 @@ public: } } + // filter poses + std::map poses; + for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) + { + poses.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i]))); + } + if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0) + { + poses = util3d::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0); + } if(assembledMapClouds_.getNumSubscribers()) { // generate the assembled cloud! pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) + for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) { - Transform pose = transformFromPoseMsg(msg->poses[i]); - - std::map::Ptr >::iterator iter = rgbClouds_.find(msg->poseIDs[i]); - if(iter != rgbClouds_.end()) + std::map::Ptr >::iterator jter = rgbClouds_.find(iter->first); + if(jter != rgbClouds_.end()) { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(iter->second, pose); + pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); *assembledCloud+=*transformed; } } @@ -214,14 +263,12 @@ public: // generate the assembled scan! pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); - for(unsigned int i=0; iposeIDs.size() && iposes.size(); ++i) + for(std::map::iterator iter = poses.begin(); iter!=poses.end(); ++iter) { - Transform pose = transformFromPoseMsg(msg->poses[i]); - - std::map::Ptr >::iterator iter = scans_.find(msg->poseIDs[i]); - if(iter != scans_.end()) + std::map::Ptr >::iterator jter = scans_.find(iter->first); + if(jter != scans_.end()) { - pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(iter->second, pose); + pcl::PointCloud::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second); *assembledCloud+=*transformed; } } @@ -240,6 +287,40 @@ public: assembledMapScans_.publish(cloudMsg); } } + + if(occupancyMapPub_.getNumSubscribers()) + { + // create the map + float xMin=0.0f, yMin=0.0f; + cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(poses, occupancyLocalMaps_, gridCellSize_, xMin, yMin, emptyCellFillingRadius_); + + 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 = msg->header.frame_id; + map.header.stamp = ros::Time::now(); + + occupancyMapPub_.publish(map); + } + } } private: @@ -248,10 +329,23 @@ private: double cloudVoxelSize_; double scanVoxelSize_; + double nodeFilteringAngle_; + double nodeFilteringRadius_; + + bool computeOccupancyGrid_; + double gridCellSize_; + double groundMaxAngle_; + int clusterMinSize_; + int emptyCellFillingRadius_; + double maxHeight_; + + std::map > occupancyLocalMaps_; // + ros::Subscriber mapDataTopic_; ros::Publisher assembledMapClouds_; ros::Publisher assembledMapScans_; + ros::Publisher occupancyMapPub_; std::map::Ptr > rgbClouds_; std::map::Ptr > scans_;