i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 10:17:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.17.5: added grid_prob_map topic (ref #269)

Browse Source
This commit is contained in:
matlabbe
2018-08-16 14:43:54 -04:00
parent 5e6796d1cd
commit 5660442565
6 changed files with 134 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+34
View File
@@ -523,6 +523,7 @@ void CoreWrapper::onInit()
setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this);
getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
getProbMapSrv_ = nh.advertiseService("get_prob_map", &CoreWrapper::getProbMapCallback, 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);
@@ -2192,6 +2193,39 @@ bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetM
return false;
}
bool CoreWrapper::getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
{
// create the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = mapsManager_.getGridProbMap(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;
}
return false;
}
bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res)
{
NODELET_INFO("rtabmap: Publishing map...");