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

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
README.md
+10 -4
View File
@@ -14,7 +14,9 @@ $ roslaunch rtabmap_ros rtabmap.launch \
depth_topic:=/zed/zed_node/depth/depth_registered \
camera_info_topic:=/zed/zed_node/rgb/camera_info \
frame_id:=base_link \
approx_sync:=false
approx_sync:=false \
wait_imu_to_init:=true \
imu_topic:=/zed_node/imu/data
```
The ROS2 equivalent is (with those [lines](https://github.com/stereolabs/zed-ros2-wrapper/blob/b512dce6ad4565f4770273995b147122e735ca0f/zed_wrapper/config/common.yaml#L58-L60) set to false to avoid TF conflicts):
@@ -28,9 +30,12 @@ $ ros2 launch rtabmap_ros rtabmap.launch.py \
depth_topic:=/zed/zed_node/depth/depth_registered \
camera_info_topic:=/zed/zed_node/rgb/camera_info \
frame_id:=base_link \
approx_sync:=false
approx_sync:=false \
wait_imu_to_init:=true \
imu_topic:=/zed/zed_node/imu/data \
qos:=1
```
`qos` (Quality of Service) argument should match the published topics QoS (1=RELIABLE, 2=BEST EFFORT). ROS1 was always RELIABLE.
# Installation
@@ -39,7 +44,7 @@ $ ros2 launch rtabmap_ros rtabmap.launch.py \
$ cd ~/ros2_ws
$ git clone https://github.com/introlab/rtabmap.git src/rtabmap
$ git clone --branch ros2 https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
$ export MAKEFLAGS="-j6" # Can be ignored if you have a lot of RAM
$ export MAKEFLAGS="-j6" # Can be ignored if you have a lot of RAM (>16GB)
$ colcon build --symlink-install
```
@@ -71,6 +76,7 @@ $ ros2 launch rtabmap_ros rtabmap.launch.py \
approx_sync:=true \
odom_topic:=/odom \
scan_topic:=/scan \
qos:=2 \
args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" \
use_sim_time:=true
```
include/rtabmap_ros/CommonDataSubscriber.h
+5
View File
@@ -248,6 +248,11 @@ private:
protected:
std::string subscribedTopicsMsg_;
int queueSize_;
rmw_qos_reliability_policy_t qosOdom_;
rmw_qos_reliability_policy_t qosImage_;
rmw_qos_reliability_policy_t qosCameraInfo_;
rmw_qos_reliability_policy_t qosScan_;
rmw_qos_reliability_policy_t qosUserData_;
private:
bool approxSync_;
include/rtabmap_ros/OdometryROS.h
+2
View File
@@ -82,6 +82,7 @@ protected:
void init(bool stereoParams, bool visParams, bool icpParams);
void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
void callbackCalled() {callbackCalled_ = true;}
rmw_qos_reliability_policy_t qos() const {return qos_;}
virtual void flushCallbacks() {};
tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
@@ -115,6 +116,7 @@ private:
bool publishTf_;
double waitForTransform_;
bool publishNullWhenLost_;
rmw_qos_reliability_policy_t qos_;
rtabmap::ParametersMap parameters_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
include/rtabmap_ros/pointcloud_to_depthimage.hpp
+2 -2
View File
@@ -57,8 +57,8 @@ private:
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg);
private:
image_transport::Publisher depthImage16Pub_;
image_transport::Publisher depthImage32Pub_;
image_transport::CameraPublisher depthImage16Pub_;
image_transport::CameraPublisher depthImage32Pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointCloudTransformedPub_;
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> pointCloudSub_;
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
launch/ros2/rtabmap.launch.py
+36 -3
View File
@@ -45,6 +45,13 @@ def launch_setup(context, *args, **kwargs):
DeclareLaunchArgument('depth', default_value=ConditionalText('false', 'true', IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true'"]))._predicate_func(context)), description=''),
DeclareLaunchArgument('subscribe_rgb', default_value=LaunchConfiguration('depth'), description=''),
DeclareLaunchArgument('args', default_value=LaunchConfiguration('rtabmap_args'), description='Can be used to pass RTAB-Map\'s parameters or other flags like --udebug and --delete_db_on_start/-d'),
DeclareLaunchArgument('qos_image', default_value=LaunchConfiguration('qos'), description='Specific QoS used for image input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_camera_info', default_value=LaunchConfiguration('qos'), description='Specific QoS used for camera info input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_scan', default_value=LaunchConfiguration('qos'), description='Specific QoS used for scan input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_odom', default_value=LaunchConfiguration('qos'), description='Specific QoS used for odometry input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_user_data', default_value=LaunchConfiguration('qos'), description='Specific QoS used for user input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_imu', default_value=LaunchConfiguration('qos'), description='Specific QoS used for imu input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_gps', default_value=LaunchConfiguration('qos'), description='Specific QoS used for gps input data: 0=system default, 1=Reliable, 2=Best Effort.'),
#These arguments should not be modified directly, see referred topics without "_relay" suffix above
DeclareLaunchArgument('rgb_topic_relay', default_value=ConditionalText(''.join([LaunchConfiguration('rgb_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('rgb_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'),
@@ -79,6 +86,8 @@ def launch_setup(context, *args, **kwargs):
parameters=[{
"approx_sync": LaunchConfiguration('approx_rgbd_sync'),
"queue_size": LaunchConfiguration('queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"depth_scale": LaunchConfiguration('depth_scale')}],
remappings=[
("rgb/image", LaunchConfiguration('rgb_topic_relay')),
@@ -109,7 +118,9 @@ def launch_setup(context, *args, **kwargs):
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
parameters=[{
"approx_sync": LaunchConfiguration('approx_rgbd_sync'),
"queue_size": LaunchConfiguration('queue_size')}],
"queue_size": LaunchConfiguration('queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info')}],
remappings=[
("left/image_rect", LaunchConfiguration('left_image_topic_relay')),
("right/image_rect", LaunchConfiguration('right_image_topic_relay')),
@@ -129,7 +140,8 @@ def launch_setup(context, *args, **kwargs):
package='rtabmap_ros', executable='rgbd_relay', output="screen",
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' == 'true'"])),
parameters=[{
"uncompress": True}],
"uncompress": True,
"qos": LaunchConfiguration('qos_image')}],
remappings=[
("rgbd_image", [LaunchConfiguration('rgbd_topic'), "/compressed"]),
([LaunchConfiguration('rgbd_topic'), "/compressed_relay"], LaunchConfiguration('rgbd_topic_relay'))],
@@ -150,6 +162,9 @@ def launch_setup(context, *args, **kwargs):
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg'),
"queue_size": LaunchConfiguration('queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"qos_imu": LaunchConfiguration('qos_imu'),
"subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
"guess_frame_id": LaunchConfiguration('odom_guess_frame_id'),
"guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
@@ -180,6 +195,9 @@ def launch_setup(context, *args, **kwargs):
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg'),
"queue_size": LaunchConfiguration('queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"qos_imu": LaunchConfiguration('qos_imu'),
"subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
"guess_frame_id": LaunchConfiguration('odom_guess_frame_id'),
"guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
@@ -211,6 +229,8 @@ def launch_setup(context, *args, **kwargs):
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg'),
"queue_size": LaunchConfiguration('queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_imu": LaunchConfiguration('qos_imu'),
"guess_frame_id": LaunchConfiguration('odom_guess_frame_id'),
"guess_min_translation": LaunchConfiguration('odom_guess_min_translation'),
"guess_min_rotation": LaunchConfiguration('odom_guess_min_rotation')}],
@@ -248,6 +268,13 @@ def launch_setup(context, *args, **kwargs):
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg'),
"queue_size": LaunchConfiguration('queue_size'),
"qos_image": LaunchConfiguration('qos_image'),
"qos_scan": LaunchConfiguration('qos_scan'),
"qos_odom": LaunchConfiguration('qos_odom'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"qos_imu": LaunchConfiguration('qos_imu'),
"qos_gps": LaunchConfiguration('qos_gps'),
"qos_user_data": LaunchConfiguration('qos_user_data'),
"scan_normal_k": LaunchConfiguration('scan_normal_k'),
"landmark_linear_variance": LaunchConfiguration('tag_linear_variance'),
"landmark_angular_variance": LaunchConfiguration('tag_angular_variance'),
@@ -290,7 +317,12 @@ def launch_setup(context, *args, **kwargs):
"odom_frame_id": LaunchConfiguration('odom_frame_id'),
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
"approx_sync": LaunchConfiguration('approx_sync'),
"queue_size": LaunchConfiguration('queue_size')
"queue_size": LaunchConfiguration('queue_size'),
"qos_image": LaunchConfiguration('qos_image'),
"qos_scan": LaunchConfiguration('qos_scan'),
"qos_odom": LaunchConfiguration('qos_odom'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"qos_user_data": LaunchConfiguration('qos_user_data')
}],
remappings=[
("rgb/image", LaunchConfiguration('rgb_topic_relay')),
@@ -361,6 +393,7 @@ def generate_launch_description():
DeclareLaunchArgument('namespace', default_value='rtabmap', description=''),
DeclareLaunchArgument('database_path', default_value='~/.ros/rtabmap.db', description='Where is the map saved/loaded.'),
DeclareLaunchArgument('queue_size', default_value='10', description=''),
DeclareLaunchArgument('qos', default_value='2', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('wait_for_transform', default_value='0.2', description=''),
DeclareLaunchArgument('rtabmap_args', default_value='', description='Backward compatibility, use "args" instead.'),
DeclareLaunchArgument('launch_prefix', default_value='', description='For debugging purpose, it fills prefix tag of the nodes, e.g., "xterm -e gdb -ex run --args"'),
launch/ros2/turtlebot3_rgbd.launch.py
+7
View File
@@ -18,11 +18,14 @@ from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
qos = LaunchConfiguration('qos')
parameters=[{
'frame_id':'base_footprint',
'use_sim_time':use_sim_time,
'subscribe_depth':True,
'qos_image':qos,
'qos_imu':qos,
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
}]
@@ -37,6 +40,10 @@ def generate_launch_description():
DeclareLaunchArgument(
'use_sim_time', default_value='true',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'qos', default_value='2',
description='QoS used for input sensor topics'),
# Nodes to launch
Node(
launch/ros2/turtlebot3_rgbd_sync.launch.py
+9 -1
View File
@@ -19,12 +19,16 @@ from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
qos = LaunchConfiguration('qos')
parameters=[{
'frame_id':'base_footprint',
'use_sim_time':use_sim_time,
'subscribe_rgbd':True,
'subscribe_scan':True,
'qos_scan':qos,
'qos_image':qos,
'qos_imu':qos,
# RTAB-Map's parameters should be strings:
'Reg/Strategy':'1',
'RGBD/NeighborLinkRefining':'True',
@@ -42,11 +46,15 @@ def generate_launch_description():
DeclareLaunchArgument(
'use_sim_time', default_value='true',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'qos', default_value='2',
description='QoS used for input sensor topics'),
# Nodes to launch
Node(
package='rtabmap_ros', executable='rgbd_sync', output='screen',
parameters=[{'approx_sync':True, 'use_sim_time':use_sim_time}],
parameters=[{'approx_sync':True, 'use_sim_time':use_sim_time, 'qos':qos}],
remappings=remappings),
Node(
launch/ros2/turtlebot3_scan.launch.py
+7
View File
@@ -17,6 +17,7 @@ from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
qos = LaunchConfiguration('qos')
parameters=[{
'frame_id':'base_footprint',
@@ -25,6 +26,8 @@ def generate_launch_description():
'subscribe_rgb':False,
'subscribe_scan':True,
'approx_sync':True,
'qos_scan':qos,
'qos_imu':qos,
'Reg/Strategy':'1',
'RGBD/NeighborLinkRefining':'True',
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
@@ -39,6 +42,10 @@ def generate_launch_description():
DeclareLaunchArgument(
'use_sim_time', default_value='true',
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'qos', default_value='2',
description='QoS used for input sensor topics'),
# Nodes to launch
Node(
package.xml
+1
View File
@@ -35,6 +35,7 @@
<build_depend>rosgraph_msgs</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>tf2</build_depend>
<build_depend>tf2_eigen</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>laser_geometry</build_depend>
<build_depend>pcl_conversions</build_depend>
src/CommonDataSubscriber.cpp
+18 -2
View File
@@ -374,6 +374,17 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
odomFrameId_ = node.declare_parameter("odom_frame_id", odomFrameId_);
rgbdCameras_ = node.declare_parameter("rgbd_cameras", rgbdCameras_);
queueSize_ = node.declare_parameter("queue_size", queueSize_);
int qosOdom = node.declare_parameter("qos_odom", 0);
int qosImage = node.declare_parameter("qos_image", 0);
int qosCameraInfo = node.declare_parameter("qos_camera_info", qosImage);
int qosScan = node.declare_parameter("qos_scan", 0);
int qosUserData = node.declare_parameter("qos_user_data", 0);
qosOdom_ = (rmw_qos_reliability_policy_t)qosOdom;
qosImage_ = (rmw_qos_reliability_policy_t)qosImage;
qosCameraInfo_ = (rmw_qos_reliability_policy_t)qosCameraInfo;
qosScan_ = (rmw_qos_reliability_policy_t)qosScan;
qosUserData_ = (rmw_qos_reliability_policy_t)qosUserData;
}
void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
@@ -452,8 +463,13 @@ void CommonDataSubscriber::setupCallbacks(rclcpp::Node & node)
RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan = %s", name_.c_str(), subscribedToScan2d_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan_cloud = %s", name_.c_str(), subscribedToScan3d_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan_descriptor = %s", name_.c_str(), subscribedToScanDescriptor_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: queue_size = %d", name_.c_str(), queueSize_);
RCLCPP_INFO(node.get_logger(), "%s: approx_sync = %s", name_.c_str(), approxSync_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: queue_size = %d", name_.c_str(), queueSize_);
RCLCPP_INFO(node.get_logger(), "%s: qos_image = %d", name_.c_str(), qosImage_);
RCLCPP_INFO(node.get_logger(), "%s: qos_camera_info = %d", name_.c_str(), qosCameraInfo_);
RCLCPP_INFO(node.get_logger(), "%s: qos_scan = %d", name_.c_str(), qosScan_);
RCLCPP_INFO(node.get_logger(), "%s: qos_odom = %d", name_.c_str(), qosOdom_);
RCLCPP_INFO(node.get_logger(), "%s: qos_user_data = %d", name_.c_str(), qosUserData_);
RCLCPP_INFO(node.get_logger(), "%s: approx_sync = %s", name_.c_str(), approxSync_?"true":"false");
subscribedToOdom_ = odomFrameId_.empty() && subscribedToOdom_;
if(subscribedToDepth_)
src/CoreWrapper.cpp
+8 -4
View File
@@ -713,7 +713,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
}
auto node = rclcpp::Node::make_shared("rtabmap");
image_transport::TransportHints hints(this);
defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport());
defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile());
RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str());
@@ -783,13 +783,17 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
}
this->set_parameters(rosParameters);
userDataAsyncSub_ = this->create_subscription<rtabmap_ros::msg::UserData>("user_data_async", 5, std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1));
int qosGPS = 0;
int qosIMU = 0;
qosGPS = this->declare_parameter("qos_gps", qosGPS);
qosIMU = this->declare_parameter("qos_imu", qosIMU);
userDataAsyncSub_ = this->create_subscription<rtabmap_ros::msg::UserData>("user_data_async", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1));
globalPoseAsyncSub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 5, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1));
gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", 5, std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1));
gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1));
#ifdef WITH_APRILTAG_MSGS
tagDetectionsSub_ = this->create_subscription<apriltag_ros::msg::AprilTagDetectionArray>("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1));
#endif
imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>("imu", 100, std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1));
imuSub_ = this->create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1));
republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>("republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1));
parametersClient_ = std::make_shared<rclcpp::SyncParametersClient>(this);
src/OdometryROS.cpp
+14 -8
View File
@@ -76,6 +76,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
publishTf_(true),
waitForTransform_(0.1), // 100 ms
publishNullWhenLost_(true),
qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0),
@@ -88,13 +89,16 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
configPath_(),
initialPose_(Transform::getIdentity())
{
odomPub_ = create_publisher<nav_msgs::msg::Odometry>("odom", 1);
odomInfoPub_ = create_publisher<rtabmap_ros::msg::OdomInfo>("odom_info", 1);
odomInfoLitePub_ = create_publisher<rtabmap_ros::msg::OdomInfo>("odom_info_lite", 1);
odomLocalMap_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_local_map", 1);
odomLocalScanMap_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_local_scan_map", 1);
odomLastFrame_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_last_frame", 1);
odomRgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("odom_rgbd_image", 1);
int qos = this->declare_parameter("qos", (int)qos_);
qos_ = (rmw_qos_reliability_policy_t)qos;
odomPub_ = create_publisher<nav_msgs::msg::Odometry>("odom", rclcpp::QoS(1).reliability(qos_));
odomInfoPub_ = create_publisher<rtabmap_ros::msg::OdomInfo>("odom_info", rclcpp::QoS(1).reliability(qos_));
odomInfoLitePub_ = create_publisher<rtabmap_ros::msg::OdomInfo>("odom_info_lite", rclcpp::QoS(1).reliability(qos_));
odomLocalMap_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_local_map", rclcpp::QoS(1).reliability(qos_));
odomLocalScanMap_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_local_scan_map", rclcpp::QoS(1).reliability(qos_));
odomLastFrame_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_last_frame", rclcpp::QoS(1).reliability(qos_));
odomRgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("odom_rgbd_image", rclcpp::QoS(1).reliability(qos_));
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -346,8 +350,10 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
{
int queueSize = 10;
this->get_parameter_or("queue_size", queueSize, queueSize);
imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", queueSize*5, std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
int qosImu = this->declare_parameter("qos_imu", (int)qos_);
imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(queueSize*5).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "odometry: Subscribing to IMU topic %s", imuSub_->get_topic_name());
RCLCPP_INFO(this->get_logger(), "odometry: qos_imu = %d", qosImu);
}
onOdomInit();
src/impl/CommonDataSubscriberDepth.cpp
+35 -35
View File
@@ -473,24 +473,24 @@ void CommonDataSubscriber::setupDepthCallbacks(
RCLCPP_INFO(node.get_logger(), "Setup depth callback");
image_transport::TransportHints hints(&node);
imageSub_.subscribe(&node, "rgb/image", hints.getTransport());
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport());
cameraInfoSub_.subscribe(&node, "rgb/camera_info");
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(queueSize).reliability(qosCameraInfo_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -501,11 +501,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
@@ -516,11 +516,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -531,7 +531,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
@@ -543,16 +543,16 @@ void CommonDataSubscriber::setupDepthCallbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -563,11 +563,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
@@ -578,11 +578,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -593,7 +593,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
@@ -604,17 +604,17 @@ void CommonDataSubscriber::setupDepthCallbacks(
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -625,12 +625,12 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
@@ -641,11 +641,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -656,7 +656,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
@@ -670,11 +670,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -685,11 +685,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
@@ -700,11 +700,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -715,7 +715,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
src/impl/CommonDataSubscriberOdom.cpp
+5 -5
View File
@@ -78,16 +78,16 @@ void CommonDataSubscriber::setupOdomCallbacks(
if(subscribeUserData || subscribeOdomInfo)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
}
else
@@ -100,13 +100,13 @@ void CommonDataSubscriber::setupOdomCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
}
}
else
{
odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", 5, std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::QoS(queueSize).reliability(qosOdom_), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
src/impl/CommonDataSubscriberRGB.cpp
+34 -34
View File
@@ -473,23 +473,23 @@ void CommonDataSubscriber::setupRGBCallbacks(
RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback");
image_transport::TransportHints hints(&node);
imageSub_.subscribe(&node, "rgb/image", hints.getTransport());
cameraInfoSub_.subscribe(&node, "rgb/camera_info");
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(queueSize).reliability(qosCameraInfo_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -500,11 +500,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
@@ -515,11 +515,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -530,7 +530,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
@@ -542,16 +542,16 @@ void CommonDataSubscriber::setupRGBCallbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -562,11 +562,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
@@ -577,11 +577,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -592,7 +592,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
@@ -603,17 +603,17 @@ void CommonDataSubscriber::setupRGBCallbacks(
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -624,12 +624,12 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
@@ -640,11 +640,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -655,7 +655,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
@@ -669,11 +669,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -684,11 +684,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
@@ -699,11 +699,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -714,7 +714,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
src/impl/CommonDataSubscriberRGBD.cpp
+22 -22
View File
@@ -558,17 +558,17 @@ void CommonDataSubscriber::setupRGBDCallbacks(
{
rgbdSubs_.resize(1);
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[0]->subscribe(&node, "rgbd_image");
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -579,7 +579,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -590,7 +590,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -601,7 +601,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
@@ -613,11 +613,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -628,7 +628,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -639,7 +639,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -650,7 +650,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
@@ -661,11 +661,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -676,7 +676,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -687,7 +687,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -698,7 +698,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
@@ -712,7 +712,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -723,7 +723,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -734,7 +734,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -745,7 +745,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
}
else
@@ -756,7 +756,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
}
else
{
rgbdSub_ = node.create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
rgbdSub_ = node.create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSize).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
src/impl/CommonDataSubscriberRGBD2.cpp
+21 -21
View File
@@ -361,17 +361,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
for(int i=0; i<2; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -382,7 +382,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -393,7 +393,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -404,7 +404,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
@@ -416,11 +416,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -431,7 +431,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -442,7 +442,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -453,7 +453,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
@@ -464,11 +464,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -479,7 +479,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -490,7 +490,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -501,7 +501,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
@@ -515,7 +515,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -526,7 +526,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -537,7 +537,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -548,7 +548,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
src/impl/CommonDataSubscriberRGBD3.cpp
+21 -21
View File
@@ -448,17 +448,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
for(int i=0; i<3; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -469,7 +469,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -480,7 +480,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -491,7 +491,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
@@ -503,11 +503,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -518,7 +518,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -529,7 +529,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -540,7 +540,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
@@ -551,11 +551,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -566,7 +566,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -577,7 +577,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -588,7 +588,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
@@ -602,7 +602,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -613,7 +613,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -624,7 +624,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
@@ -636,7 +636,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
src/impl/CommonDataSubscriberRGBD4.cpp
+21 -21
View File
@@ -416,17 +416,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
for(int i=0; i<4; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -437,7 +437,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -448,7 +448,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -459,7 +459,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
@@ -471,11 +471,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -486,7 +486,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -497,7 +497,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -508,7 +508,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
@@ -519,11 +519,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -534,7 +534,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -545,7 +545,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -556,7 +556,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
@@ -570,7 +570,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -581,7 +581,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -592,7 +592,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -603,7 +603,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
src/impl/CommonDataSubscriberRGBD5.cpp
+10 -10
View File
@@ -267,15 +267,15 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
for(int i=0; i<5; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
}
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -286,7 +286,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -297,7 +297,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -308,7 +308,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
}
else
@@ -321,7 +321,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -332,7 +332,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -343,7 +343,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -354,7 +354,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
}
else
src/impl/CommonDataSubscriberRGBD6.cpp
+10 -10
View File
@@ -284,15 +284,15 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
for(int i=0; i<6; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
}
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -303,7 +303,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -314,7 +314,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -325,7 +325,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
@@ -338,7 +338,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -349,7 +349,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -360,7 +360,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -371,7 +371,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
src/impl/CommonDataSubscriberRGBDX.cpp
+22 -22
View File
@@ -334,16 +334,16 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
{
RCLCPP_INFO(node.get_logger(), "Setup rgbdX callback");
rgbdXSub_.subscribe(&node, "rgbd_images");
rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -354,7 +354,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -365,7 +365,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -376,7 +376,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
}
else
@@ -388,11 +388,11 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -403,7 +403,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -414,7 +414,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -425,7 +425,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_);
}
else
@@ -436,11 +436,11 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -451,7 +451,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -462,7 +462,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -473,7 +473,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_);
}
else
@@ -487,7 +487,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -498,7 +498,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -509,7 +509,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
@@ -520,12 +520,12 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_);
}
else
{
rgbdXSubOnly_ = node.create_subscription<rtabmap_ros::msg::RGBDImages>("rgbd_images", 5, std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
rgbdXSubOnly_ = node.create_subscription<rtabmap_ros::msg::RGBDImages>("rgbd_images", rclcpp::QoS(queueSize).reliability(qosOdom_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
src/impl/CommonDataSubscriberScan.cpp
+20 -20
View File
@@ -292,31 +292,31 @@ void CommonDataSubscriber::setupScanCallbacks(
if(scanDescTopic)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
}
else if(scan2dTopic)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
scanSub_.subscribe(&node, "scan", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
}
else
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -329,7 +329,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
}
else
@@ -342,7 +342,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -355,14 +355,14 @@ void CommonDataSubscriber::setupScanCallbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -375,7 +375,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
}
else
@@ -388,7 +388,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -400,14 +400,14 @@ void CommonDataSubscriber::setupScanCallbacks(
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(queueSize).reliability(qosUserData_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
}
else
@@ -420,7 +420,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_);
}
else
@@ -433,7 +433,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
}
else
@@ -446,7 +446,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(scanDescTopic)
{
SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
@@ -466,7 +466,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(scanDescTopic)
{
subscribedToScanDescriptor_ = true;
scanDescSubOnly_ = node.create_subscription<rtabmap_ros::msg::ScanDescriptor>("scan_descriptor", 5, std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1));
scanDescSubOnly_ = node.create_subscription<rtabmap_ros::msg::ScanDescriptor>("scan_descriptor", rclcpp::QoS(queueSize).reliability(qosScan_), std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
@@ -475,7 +475,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else if(scan2dTopic)
{
subscribedToScan2d_ = true;
scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", 5, std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(queueSize).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
@@ -484,7 +484,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else
{
subscribedToScan3d_ = true;
scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", 5, std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(queueSize).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
src/impl/CommonDataSubscriberStereo.cpp
+7 -7
View File
@@ -99,19 +99,19 @@ void CommonDataSubscriber::setupStereoCallbacks(
RCLCPP_INFO(node.get_logger(), "Setup stereo callback");
image_transport::TransportHints hints(&node);
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport());
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport());
cameraInfoLeft_.subscribe(&node, "left/camera_info");
cameraInfoRight_.subscribe(&node, "right/camera_info");
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(queueSize).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(queueSize).reliability(qosCameraInfo_).get_rmw_qos_profile());
cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(queueSize).reliability(qosCameraInfo_).get_rmw_qos_profile());
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom");
odomSub_.subscribe(&node, "odom", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
@@ -124,7 +124,7 @@ void CommonDataSubscriber::setupStereoCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(queueSize).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
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(),