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

Added "cloud_ceiling_culling_height" parameter to MapsManager and MapCloud rviz plugin

Browse Source
This commit is contained in:
matlabbe
2015-11-27 14:11:11 -05:00
parent 8eea06c32b
commit 736a909725
4 changed files with 26 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+13 -2
View File
@@ -34,6 +34,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
cloudMaxDepth_(4.0), // meters
cloudVoxelSize_(0.05), // meters
cloudFloorCullingHeight_(0.0),
cloudCeilingCullingHeight_(0.0),
cloudOutputVoxelized_(false),
cloudFrustumCulling_(false),
cloudNoiseFilteringRadius_(0.0),
@@ -60,6 +61,14 @@ MapsManager::MapsManager(bool usePublicNamespace) :
pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
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_);
@@ -504,9 +513,11 @@ void MapsManager::publishMaps(
}
}
if(assembledCloud->size() && cloudFloorCullingHeight_ > 0.0)
if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0))
{
assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f);
assembledCloud = util3d::passThrough(assembledCloud, "z",
cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0,
cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0);
}
if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
src/MapsManager.h
+1
View File
@@ -70,6 +70,7 @@ private:
double cloudMaxDepth_;
double cloudVoxelSize_;
double cloudFloorCullingHeight_;
double cloudCeilingCullingHeight_;
bool cloudOutputVoxelized_;
bool cloudFrustumCulling_;
double cloudNoiseFilteringRadius_;
src/rviz/MapCloudDisplay.cpp
+11 -2
View File
@@ -156,6 +156,13 @@ MapCloudDisplay::MapCloudDisplay()
cloud_filter_floor_height_->setMin( 0.0f );
cloud_filter_floor_height_->setMax( 999.0f );
cloud_filter_ceiling_height_ = new rviz::FloatProperty( "Filter ceiling (m)", 0.0f,
"Filter the ceiling at the specified height set here "
"(only appropriate for 2D mapping).",
this, SLOT( updateCloudParameters() ), this );
cloud_filter_ceiling_height_->setMin( 0.0f );
cloud_filter_ceiling_height_->setMax( 999.0f );
node_filtering_radius_ = new rviz::FloatProperty( "Node filtering radius (m)", 0.2f,
"(Disabled=0) Only keep one node in the specified radius.",
this, SLOT( updateCloudParameters() ), this );
@@ -276,9 +283,11 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
if(cloud->size())
{
if(cloud_filter_floor_height_->getFloat() > 0.0f)
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
cloud = rtabmap::util3d::passThrough(cloud, "z",
cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
src/rviz/MapCloudDisplay.h
+1
View File
@@ -108,6 +108,7 @@ public:
rviz::FloatProperty* cloud_max_depth_;
rviz::FloatProperty* cloud_voxel_size_;
rviz::FloatProperty* cloud_filter_floor_height_;
rviz::FloatProperty* cloud_filter_ceiling_height_;
rviz::FloatProperty* node_filtering_radius_;
rviz::FloatProperty* node_filtering_angle_;
rviz::BoolProperty* download_map_;