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

Merged master to ros2. ros2: Uniformized qos of all subscribers and publishers.

Browse Source
This commit is contained in:
matlabbe
2021-10-04 19:29:19 -04:00
parent ccf152d877 1ecce15878
commit af1c7edeef
121 changed files with 10854 additions and 3478 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/point_cloud_xyzrgb.cpp
+12 -11
View File
@@ -130,7 +130,7 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 1);
rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
if(approxSync)
{
@@ -157,16 +157,16 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoSub_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
imageSub_.subscribe(this, "rgb/image", hints.getTransport());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport());
cameraInfoSub_.subscribe(this, "rgb/camera_info");
imageDisparitySub_.subscribe(this, "disparity", rmw_qos_profile_sensor_data);
imageDisparitySub_.subscribe(this, "disparity");
imageLeft_.subscribe(this, "left/image", hints.getTransport(), rmw_qos_profile_sensor_data);
imageRight_.subscribe(this, "right/image", hints.getTransport(), rmw_qos_profile_sensor_data);
cameraInfoLeft_.subscribe(this, "left/camera_info", rmw_qos_profile_sensor_data);
cameraInfoRight_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);
imageLeft_.subscribe(this, "left/image", hints.getTransport());
imageRight_.subscribe(this, "right/image", hints.getTransport());
cameraInfoLeft_.subscribe(this, "left/camera_info");
cameraInfoRight_.subscribe(this, "right/camera_info");
}
PointCloudXYZRGB::~PointCloudXYZRGB()
@@ -407,10 +407,11 @@ void PointCloudXYZRGB::processAndPublish(
if(indices->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
pclCloud->is_dense = true;
}
// Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(pclCloud->is_dense)
{
@@ -426,7 +427,7 @@ void PointCloudXYZRGB::processAndPublish(
}
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);