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

rtabmap: added local grid output topics for convenience. rviz/MapCloud: added cloud_from_scan option. point_cloud_assembler: added option to skip clouds.

Browse Source
This commit is contained in:
matlabbe
2019-05-02 17:03:04 -04:00
parent e3abe460ee
commit b670186610
5 changed files with 145 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/MapCloudDisplay.cpp
+65 -23
View File
@@ -131,6 +131,10 @@ MapCloudDisplay::MapCloudDisplay()
connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
this, SLOT( setColorTransformerOptions( EnumProperty* )));
cloud_from_scan_ = new rviz::BoolProperty( "Cloud from scan", false,
"Create the cloud from laser scans instead of the RGB-D/Stereo images.",
this, SLOT( updateCloudParameters() ), this );
cloud_decimation_ = new rviz::IntProperty( "Cloud decimation", 4,
"Decimation of the input RGB and depth images before creating the cloud.",
this, SLOT( updateCloudParameters() ), this );
@@ -269,24 +273,31 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
}
// Add new clouds...
bool fromDepth = !cloud_from_scan_->getBool();
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{
int id = map.nodes[i].id;
// Always refresh the cloud if there are data
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection()))
ROS_WARN("s.sensorData().laserScanCompressed()=%d", s.sensorData().laserScanCompressed().size());
if((fromDepth &&
!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection())) ||
(!fromDepth && !s.sensorData().laserScanCompressed().isEmpty()))
{
cv::Mat image, depth;
s.sensorData().uncompressData(&image, &depth, 0);
rtabmap::LaserScan scan;
s.sensorData().uncompressData(fromDepth?&image:0, fromDepth?&depth:0, !fromDepth?&scan:0);
if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
if(fromDepth && !s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr validIndices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(
s.sensorData(),
cloud_decimation_->getInt(),
@@ -294,13 +305,13 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
cloud_min_depth_->getFloat(),
validIndices.get());
if(cloud_voxel_size_->getFloat())
if(!cloud->empty())
{
cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
}
if(cloud_voxel_size_->getFloat())
{
cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
}
if(cloud->size())
{
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
{
// convert in /odom frame
@@ -312,23 +323,54 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cloud, *cloudMsg);
cloudMsg->header = map.header;
CloudInfoPtr info(new CloudInfo);
info->message_ = cloudMsg;
info->pose_ = rtabmap::Transform::getIdentity();
info->id_ = id;
if (transformCloud(info, true))
if(!cloud->empty())
{
boost::mutex::scoped_lock lock(new_clouds_mutex_);
new_cloud_infos_.erase(id);
new_cloud_infos_.insert(std::make_pair(id, info));
pcl::toROSMsg(*cloud, *cloudMsg);
}
}
}
else if(!fromDepth && !scan.isEmpty())
{
scan = rtabmap::util3d::commonFiltering(
scan,
1,
cloud_min_depth_->getFloat(),
cloud_max_depth_->getFloat(),
cloud_voxel_size_->getFloat());
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
cloud = rtabmap::util3d::laserScanToPointCloudI(scan, scan.localTransform());
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
{
// convert in /odom frame
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose());
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);
// convert back in /base_link frame
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
}
if(!cloud->empty())
{
pcl::toROSMsg(*cloud, *cloudMsg);
}
}
if(!cloudMsg->data.empty())
{
cloudMsg->header = map.header;
CloudInfoPtr info(new CloudInfo);
info->message_ = cloudMsg;
info->pose_ = rtabmap::Transform::getIdentity();
info->id_ = id;
if (transformCloud(info, true))
{
boost::mutex::scoped_lock lock(new_clouds_mutex_);
new_cloud_infos_.erase(id);
new_cloud_infos_.insert(std::make_pair(id, info));
}
}
}
}
src/rviz/MapCloudDisplay.h
+1
View File
@@ -108,6 +108,7 @@ public:
rviz::EnumProperty* xyz_transformer_property_;
rviz::EnumProperty* color_transformer_property_;
rviz::EnumProperty* style_property_;
rviz::BoolProperty* cloud_from_scan_;
rviz::IntProperty* cloud_decimation_;
rviz::FloatProperty* cloud_max_depth_;
rviz::FloatProperty* cloud_min_depth_;