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

Fixed tf2_eigen missing from package.xml. Added "qos" parameter for all nodes subscribing to sensor data (#651).

Browse Source
This commit is contained in:
matlabbe
2021-11-08 18:36:19 -05:00
parent c94d3a6a92
commit b606a54f45
38 changed files with 472 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/icp_odometry.cpp
+7 -3
View File
@@ -71,6 +71,8 @@ ICPOdometry::~ICPOdometry()
void ICPOdometry::onOdomInit()
{
int queueSize = 1;
queueSize = this->declare_parameter("queue_size", queueSize);
scanCloudMaxPoints_ = this->declare_parameter("scan_cloud_max_points", scanCloudMaxPoints_);
scanDownsamplingStep_ = this->declare_parameter("scan_downsampling_step", scanDownsamplingStep_);
scanRangeMin_ = this->declare_parameter("scan_range_min", scanRangeMin_);
@@ -109,6 +111,8 @@ void ICPOdometry::onOdomInit()
}
}*/
RCLCPP_INFO(this->get_logger(), "IcpOdometry: queue_size = %d", queueSize);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_min = %f m", scanRangeMin_);
@@ -118,10 +122,10 @@ void ICPOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_);
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", 5, std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", 5, std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", 1);
filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
}
void ICPOdometry::updateParameters(ParametersMap & parameters)
src/nodelets/obstacles_detection.cpp
+6 -4
View File
@@ -52,6 +52,8 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
frameId_ = this->declare_parameter("frame_id", frameId_);
mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
int qos = 0;
qos = this->declare_parameter("qos", qos);
rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
@@ -76,11 +78,11 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock());
tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_);
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", 5, std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
groundPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("ground", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
obstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("obstacles", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
projObstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("proj_obstacles", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
groundPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("ground", 5);
obstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("obstacles", 5);
projObstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("proj_obstacles", 5);
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
}
src/nodelets/point_cloud_aggregator.cpp
+8 -6
View File
@@ -61,23 +61,25 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
int queueSize = 5;
int count = 2;
bool approx=true;
int qos;
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("queue_size", qos);
frameId_ = this->declare_parameter("frame_id", frameId_);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
approx = this->declare_parameter("approx_sync", approx);
count = this->declare_parameter("count", count);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", 1);
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cloudSub_1_.subscribe(this, "cloud1");
cloudSub_2_.subscribe(this, "cloud2");
cloudSub_1_.subscribe(this, "cloud1", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_2_.subscribe(this, "cloud2", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
std::string subscribedTopicsMsg;
if(count == 4)
{
cloudSub_3_.subscribe(this, "cloud3");
cloudSub_4_.subscribe(this, "cloud4");
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_4_.subscribe(this, "cloud4", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
if(approx)
{
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
@@ -98,7 +100,7 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
}
else if(count == 3)
{
cloudSub_3_.subscribe(this, "cloud3");
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
if(approx)
{
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
src/nodelets/point_cloud_assembler.cpp
+12 -7
View File
@@ -74,9 +74,12 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int queueSize = 5;
int qos = 0;
bool subscribeOdomInfo = false;
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosOdom = this->declare_parameter("qos_odom", qos);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
frameId_ = this->declare_parameter("frame_id", frameId_);
maxClouds_ = this->declare_parameter("max_clouds", maxClouds_);
@@ -95,6 +98,8 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
subscribeOdomInfo = this->declare_parameter("subscribe_odom_info", subscribeOdomInfo);
RCLCPP_INFO(this->get_logger(), "%s: queue_size=%d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos=%d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_odom=%d", get_name(), qosOdom);
RCLCPP_INFO(this->get_logger(), "%s: fixed_frame_id=%s", get_name(), fixedFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), "%s: frame_id=%s", get_name(), frameId_.c_str());
RCLCPP_INFO(this->get_logger(), "%s: max_clouds=%d", get_name(), maxClouds_);
@@ -119,20 +124,20 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
cloudsSkipped_ = skipClouds_;
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("assembled_cloud", 1);
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("assembled_cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos));
if(!fixedFrameId_.empty())
{
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", 5, std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to %s",
get_name(),
cloudSub_->get_topic_name());
}
else if(subscribeOdomInfo)
{
syncCloudSub_.subscribe(this, "cloud");
syncOdomSub_.subscribe(this, "odom");
syncOdomInfoSub_.subscribe(this, "odom_info");
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
syncOdomInfoSub_.subscribe(this, "odom_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
exactInfoSync_->registerCallback(std::bind(&rtabmap_ros::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
@@ -143,8 +148,8 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
}
else
{
syncCloudSub_.subscribe(this, "cloud");
syncOdomSub_.subscribe(this, "odom");
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
exactSync_->registerCallback(std::bind(&rtabmap_ros::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
src/nodelets/point_cloud_xyz.cpp
+8 -5
View File
@@ -63,10 +63,13 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
exactSyncDisparity_(0)
{
int queueSize = 10;
int qos = 0;
bool approxSync = true;
std::string roiStr;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
minDepth_ = this->declare_parameter("min_depth", minDepth_);
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
@@ -130,14 +133,14 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 1);
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
image_transport::TransportHints hints(this);
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport());
cameraInfoSub_.subscribe(this, "depth/camera_info");
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "depth/camera_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
disparitySub_.subscribe(this, "disparity/image");
disparityCameraInfoSub_.subscribe(this, "disparity/camera_info");
disparitySub_.subscribe(this, "disparity/image", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudXYZ::~PointCloudXYZ()
src/nodelets/point_cloud_xyzrgb.cpp
+13 -10
View File
@@ -68,8 +68,11 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
bool approxSync = true;
std::string roiStr;
int queueSize = 10;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
minDepth_ = this->declare_parameter("min_depth", minDepth_);
voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
@@ -128,9 +131,9 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "Approximate time sync = %s", approxSync?"true":"false");
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 1);
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
if(approxSync)
{
@@ -157,16 +160,16 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport());
cameraInfoSub_.subscribe(this, "rgb/camera_info");
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageDisparitySub_.subscribe(this, "disparity");
imageDisparitySub_.subscribe(this, "disparity", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
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");
imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudXYZRGB::~PointCloudXYZRGB()
src/nodelets/pointcloud_to_depthimage.cpp
+10 -7
View File
@@ -59,8 +59,11 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int queueSize = 10;
int qos = 0;
bool approx = true;
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
fillHolesSize_ = this->declare_parameter("fill_holes_size", fillHolesSize_);
@@ -89,9 +92,9 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
auto node = rclcpp::Node::make_shared(this->get_name());
image_transport::ImageTransport it(node);
depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
depthImage32Pub_ = it.advertise("image", 1);// 32 bits float in meters
pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", 1);
depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
if(approx)
{
@@ -105,8 +108,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
exactSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
}
pointCloudSub_.subscribe(this, "cloud");
cameraInfoSub_.subscribe(this, "camera_info");
pointCloudSub_.subscribe(this, "cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "camera_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudToDepthImage::~PointCloudToDepthImage()
@@ -212,14 +215,14 @@ void PointCloudToDepthImage::callback(
if(depthImage32Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depthImage32Pub_.publish(depthImage.toImageMsg());
depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
}
if(depthImage16Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
depthImage16Pub_.publish(depthImage.toImageMsg());
depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
}
if( cloudStamp != timestampFromROS(pointCloud2Msg->header.stamp) ||
src/nodelets/rgb_sync.cpp
+9 -4
View File
@@ -51,16 +51,21 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
{
int queueSize = 10;
bool approxSync = true;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCaminfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo);
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
rgbdImagePub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", 1);
rgbdImagePub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
if(approxSync)
{
@@ -74,8 +79,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport());
cameraInfoSub_.subscribe(this, "rgb/camera_info");
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s",
get_name(),
src/nodelets/rgbd_odometry.cpp
+14 -11
View File
@@ -81,6 +81,7 @@ void RGBDOdometry::onOdomInit()
bool subscribeRGBD = false;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
if(rgbdCameras <= 0)
@@ -95,6 +96,8 @@ void RGBDOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
@@ -104,19 +107,19 @@ void RGBDOdometry::onOdomInit()
{
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0");
rgbd_image2_sub_.subscribe(this, "rgbd_image1");
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2");
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3");
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4");
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras == 2)
@@ -210,7 +213,7 @@ void RGBDOdometry::onOdomInit()
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
rgbd_image5_sub_);
approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
else
@@ -221,7 +224,7 @@ void RGBDOdometry::onOdomInit()
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
@@ -236,7 +239,7 @@ void RGBDOdometry::onOdomInit()
}
else
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", queueSize_, std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
@@ -247,9 +250,9 @@ void RGBDOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport());
info_sub_.subscribe(this, "rgb/camera_info");
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
if(approxSync)
{
src/nodelets/rgbd_relay.cpp
+4 -2
View File
@@ -48,11 +48,13 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
compress_(false),
uncompress_(false)
{
int qos = 0;
qos = this->declare_parameter("qos", qos);
compress_ = this->declare_parameter("compress", compress_);
uncompress_ = this->declare_parameter("uncompress", uncompress_);
rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
rgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image_relay", 1);
rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
rgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
}
void RGBDRelay::callback(const rtabmap_ros::msg::RGBDImage::SharedPtr input) const
src/nodelets/rgbd_sync.cpp
+10 -5
View File
@@ -53,8 +53,11 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
{
int queueSize = 10;
bool approxSync = true;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
depthScale_ = this->declare_parameter("depth_scale", depthScale_);
decimation_ = this->declare_parameter("decimation", decimation_);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
@@ -66,12 +69,14 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
RCLCPP_INFO(this->get_logger(), "%s: depth_scale = %f", get_name(), depthScale_);
RCLCPP_INFO(this->get_logger(), "%s: decimation = %d", get_name(), decimation_);
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
rgbdImagePub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", 1);
rgbdImagePub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageCompressedPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
if(approxSync)
{
@@ -85,9 +90,9 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport());
cameraInfoSub_.subscribe(this, "rgb/camera_info");
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
get_name(),
src/nodelets/rgbdx_sync.cpp
+5 -2
View File
@@ -47,15 +47,18 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
int queueSize = 10;
bool approxSync = true;
int rgbdCameras = 2;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: rgbd_cameras = %d", get_name(), rgbdCameras);
rgbdImagesPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImages>("rgbd_images", 1);
rgbdImagesPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
UASSERT(rgbdCameras>=2 && rgbdCameras<=8);
@@ -63,7 +66,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
for(int i=0; i<rgbdCameras; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i));
rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
}
std::string name_ = get_name();
src/nodelets/stereo_odometry.cpp
+8 -5
View File
@@ -68,18 +68,21 @@ void StereoOdometry::onOdomInit()
bool subscribeRGBD = false;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
keepColor_ = this->declare_parameter("keep_color", keepColor_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false");
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
@@ -89,10 +92,10 @@ void StereoOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport());
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport());
cameraInfoLeft_.subscribe(this, "left/camera_info");
cameraInfoRight_.subscribe(this, "right/camera_info");
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
if(approxSync)
{
src/nodelets/stereo_sync.cpp
+11 -6
View File
@@ -50,16 +50,21 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
{
int queueSize = 10;
bool approxSync = false;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
rgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", 1);
rgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageCompressedPub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
if(approxSync)
{
@@ -73,10 +78,10 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
}
image_transport::TransportHints hints(this);
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport());
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport());
cameraInfoLeftSub_.subscribe(this, "left/camera_info");
cameraInfoRightSub_.subscribe(this, "right/camera_info");
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
get_name(),