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

GridMapAssembler ros node: updated interface of util3d::create2DMap()

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1454 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-03 20:17:48 +00:00
parent f302e18403
commit f3235ab176
1 changed files with 21 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GridMapAssemblerNode.cpp
+21 -7
View File
@@ -43,10 +43,18 @@ class GridMapAssembler
public:
GridMapAssembler() :
scanVoxelSize_(0.01)
gridCellSize_(0.05),
gridUnknownSpaceFilled_(true),
filterRadius_(0.5),
filterAngle_(30.0)
{
ros::NodeHandle pnh("~");
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("cell_size", gridCellSize_, gridCellSize_);
pnh.param("unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
pnh.param("filter_radius", filterRadius_, filterRadius_);
pnh.param("filter_angle", filterAngle_, filterAngle_);
UASSERT(gridCellSize_ > 0.0);
ros::NodeHandle nh;
infoExTopic_ = nh.subscribe("infoEx", 1, &GridMapAssembler::infoExReceivedCallback, this);
@@ -77,18 +85,21 @@ public:
poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i])));
}
if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
{
poses = util3d::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
}
if(gridMap_.getNumSubscribers())
{
float delta = 0.05; //m
// create the map
float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMap(poses, scans_, delta, xMin, yMin);
cv::Mat pixels = util3d::create2DMap(poses, scans_, gridCellSize_, gridUnknownSpaceFilled_, xMin, yMin);
if(!pixels.empty())
{
//init
map_.info.resolution = delta;
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;
@@ -145,7 +156,10 @@ public:
}
private:
double scanVoxelSize_;
double gridCellSize_;
bool gridUnknownSpaceFilled_;
double filterRadius_;
double filterAngle_;
ros::Subscriber infoExTopic_;
ros::Subscriber mapDataTopic_;