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

Merge branch 'ros2' of github.com:introlab/rtabmap_ros into jazzy-devel

Browse Source
This commit is contained in:
matlabbe
2024-06-30 19:52:48 -07:00
parent a526abe5c5 57dd787a58
commit 616ad5c028
58 changed files with 1389 additions and 952 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.devcontainer/humble/devcontainer.json
+11
View File
@@ -0,0 +1,11 @@
{
"image": "introlab3it/rtabmap_ros:humble-latest",
"customizations": {
"vscode": {
"extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
}
},
"workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
"workspaceFolder": "/ros2_ws",
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/humble/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'"
}
.devcontainer/jazzy/devcontainer.json
+11
View File
@@ -0,0 +1,11 @@
{
"image": "introlab3it/rtabmap_ros:jazzy-latest",
"customizations": {
"vscode": {
"extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools", "vscjava.vscode-java-pack"]
}
},
"workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
"workspaceFolder": "/ros2_ws",
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/jazzy/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'"
}
.github/workflows/docker-ros2.yml
+11 -1
View File
@@ -11,7 +11,7 @@ jobs:
strategy:
matrix:
docker_tag: [humble, humble-latest, iron, iron-latest]
docker_tag: [humble, humble-latest, iron, iron-latest, jazzy-latest]
include:
- docker_tag: humble
docker_path: 'humble'
@@ -30,6 +30,16 @@ jobs:
docker_path: 'iron/latest'
docker_platforms: |
linux/amd64
# Re-add "jazzy" after binaries are released
# - docker_tag: jazzy
# docker_path: 'jazzy'
# docker_platforms: |
# linux/amd64
- docker_tag: jazzy-latest
docker_path: 'jazzy/latest'
docker_platforms: |
linux/amd64
linux/arm64
steps:
-
.github/workflows/ros2.yml
+19 -29
View File
@@ -17,42 +17,32 @@ jobs:
# well on Windows or Mac. You can convert this to a matrix build if you need
# cross-platform coverage.
# See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
name: Build on ros2 ${{ matrix.ros_distro }} and ${{ matrix.os }}
runs-on: ${{ matrix.os }}
name: Build ros2 ${{ matrix.ros_distro }} on ubuntu ${{ matrix.ubuntu_distro }}
runs-on: ubuntu-latest
strategy:
matrix:
ros_distro: [humble, iron]
include:
- ros_distro: 'humble'
os: ubuntu-22.04
ubuntu_distro: 'jammy'
- ros_distro: 'iron'
os: ubuntu-22.04
ubuntu_distro: 'jammy'
# Disabled as there still missing dependencies on jazzy:
# - ros_distro: 'jazzy'
# ubuntu_distro: 'noble'
fail-fast: false
container:
image: rostooling/setup-ros-docker:ubuntu-${{ matrix.ubuntu_distro }}-ros-${{ matrix.ros_distro }}-desktop-latest
steps:
- uses: ros-tooling/[email protected]
- uses: actions/checkout@v4
- uses: ros-tooling/[email protected]
with:
required-ros-distributions: ${{ matrix.ros_distro }}
- name: Setup ros2 workspace
run: |
source /opt/ros/${{ matrix.ros_distro }}/setup.bash
mkdir -p ${{github.workspace}}/ros2_ws/src
cd ${{github.workspace}}/ros2_ws
colcon build
- uses: actions/checkout@v2
- run: |
echo "repositories:\n introlab/rtabmap:\n type: git\n url: https://github.com/introlab/rtabmap.git\n version: master\n" > /tmp/deps.repos &&\
cat /tmp/deps.repos
- uses: ros-tooling/[email protected]
with:
repository: 'introlab/rtabmap'
path: 'ros2_ws/src/rtabmap'
- uses: actions/checkout@v2
with:
path: 'ros2_ws/src/rtabmap_ros'
- name: colcon build
run: |
source /opt/ros/${{ matrix.ros_distro }}/setup.bash
cd ${{github.workspace}}/ros2_ws
rosdep update
rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool
colcon build --event-handlers console_direct+
package-name: rtabmap_ros
target-ros2-distro: ${{ matrix.ros_distro }}
vcs-repo-file-url: /tmp/deps.repos
docker/README.md
+2
View File
@@ -13,6 +13,7 @@
docker run -it --rm \
--user $UID \
-e ROS_HOME=/tmp/.ros \
-e OMP_WAIT_POLICY=passive \
--network=host \
--ipc=host \
-v ~/.ros:/tmp/.ros \
@@ -35,6 +36,7 @@
-e NVIDIA_VISIBLE_DEVICES=all \
-e NVIDIA_DRIVER_CAPABILITIES=all \
-e XAUTHORITY=$XAUTH \
-e OMP_WAIT_POLICY=passive \
--user $UID \
-e ROS_HOME=/tmp/.ros \
-v ~/.ros:/tmp/.ros \
docker/jazzy/Dockerfile
+7
View File
@@ -0,0 +1,7 @@
FROM osrf/ros:jazzy-desktop
# install rtabmap packages
ARG CACHE_DATE=2016-01-01
RUN apt-get update && apt-get install -y \
ros-jazzy-rtabmap \
ros-jazzy-rtabmap-ros \
&& rm -rf /var/lib/apt/lists/
docker/jazzy/latest/Dockerfile
+19
View File
@@ -0,0 +1,19 @@
FROM introlab3it/rtabmap:24.04
RUN source /ros_entrypoint.sh && \
mkdir -p ros2_ws/src && \
cd ros2_ws/src
COPY . ros2_ws/src/rtabmap_ros
RUN source /ros_entrypoint.sh && \
cd ros2_ws && \
export MAKEFLAGS="-j1" && \
rosdep init && \
rosdep update && \
apt-get update && \
rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
apt-get clean && rm -rf /var/lib/apt/lists/ && \
colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
cd && \
rm -rf ros2_ws
rtabmap_conversions/CMakeLists.txt
+8
View File
@@ -46,6 +46,10 @@ if("$ENV{ROS_DISTRO}" STRLESS "humble")
add_definitions(-DPRE_ROS_HUMBLE)
endif()
IF("$ENV{ROS_DISTRO}" STRLESS "iron")
add_definitions(-DPRE_ROS_IRON)
ENDIF()
###########
## Build ##
###########
@@ -58,6 +62,10 @@ target_include_directories(rtabmap_conversions
)
ament_target_dependencies(rtabmap_conversions ${Libraries})
IF("$ENV{ROS_DISTRO}" STRLESS "iron")
target_compile_definitions(rtabmap_conversions PUBLIC -DPRE_ROS_IRON)
ENDIF()
#############
## Install ##
#############
rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
+4
View File
@@ -40,7 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/opencv.hpp>
#include <opencv2/features2d/features2d.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/Link.h>
rtabmap_conversions/src/MsgConversion.cpp
+6 -1
View File
@@ -38,8 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl_conversions/pcl_conversions.h>
#ifdef PRE_ROS_IRON
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#else
#include <image_geometry/pinhole_camera_model.hpp>
#include <image_geometry/stereo_camera_model.hpp>
#endif
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/point_field.hpp>
#include <geometry_msgs/msg/transform.hpp>
@@ -47,7 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_surface.h>
#ifdef PRE_ROS_HUMBLE
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py
+80
View File
@@ -0,0 +1,80 @@
#
# Note: Make sure you have this fix for turtlebot4_description https://github.com/turtlebot/turtlebot4/pull/434,
# otherwise, the lidar and camera point cloud won't be aligned correctly.
#
# Example:
# 1) Launch simulator (turtlebot4, nav2 and rtabmap):
# $ ros2 launch rtabmap_demos turtlebot4_ignition.launch.py
#
# 2) Click on "Play" button on bottom-left of gazebo.
#
# 3) Click on double points ".." button on top-right next to power button to undock.
#
# 4) Move the robot:
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
# a) By teleoperating:
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
# c) By using autonomous exploration node (tested with https://github.com/robo-friends/m-explore-ros2):
# $ ros2 launch explore_lite explore.launch.py
#
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
ARGUMENTS = [
DeclareLaunchArgument('rviz', default_value='true',
choices=['true', 'false'], description='Start rviz.'),
DeclareLaunchArgument('rtabmap_viz', default_value='true',
choices=['true', 'false'], description='Start rtabmap_viz.'),
DeclareLaunchArgument('localization', default_value='false',
choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'),
DeclareLaunchArgument('nav2', default_value='true',
choices=['true', 'false'], description='Start nav2.'),
DeclareLaunchArgument('world', default_value='warehouse',
description='Ignition World'),
]
def generate_launch_description():
# Directories
pkg_turtlebot4_ignition_bringup = get_package_share_directory(
'turtlebot4_ignition_bringup')
pkg_rtabmap_demos = get_package_share_directory(
'rtabmap_demos')
# Paths
ignition_launch = PathJoinSubstitution(
[pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_ignition.launch.py'])
rtabmap_launch = PathJoinSubstitution(
[pkg_rtabmap_demos, 'launch', 'turtlebot4_slam.launch.py'])
ignition = IncludeLaunchDescription(
PythonLaunchDescriptionSource([ignition_launch]),
launch_arguments=[
('world', LaunchConfiguration('world')),
('slam', 'false'),
('localization', 'false'),
('nav2', LaunchConfiguration('nav2')),
('rviz', LaunchConfiguration('rviz'))
]
)
rtabmap = IncludeLaunchDescription(
PythonLaunchDescriptionSource([rtabmap_launch]),
launch_arguments=[
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
('localization', LaunchConfiguration('localization')),
('qos', '2'),
('use_sim_time', 'true')
]
)
# Create launch description and add actions
ld = LaunchDescription(ARGUMENTS)
ld.add_action(rtabmap) # put it first so that localization arg is not overwritten by the same used by ignition
ld.add_action(ignition)
return ld
rtabmap_demos/launch/turtlebot4_slam.launch.py
+122
View File
@@ -0,0 +1,122 @@
#
# Note: Make sure you have this fix for turtlebot4_description https://github.com/turtlebot/turtlebot4/pull/434,
# otherwise, the lidar and camera point cloud won't be aligned correctly.
#
# Example with gazebo:
# 1) Launch simulator (turtlebot4 and nav2):
# $ ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py slam:=false nav2:=true rviz:=true
#
# 2) Launch SLAM:
# $ ros2 launch rtabmap_demos turtlebot4_slam.launch.py use_sim_time:=true qos:=2
# OR
# $ ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=true subscribe_scan:=true rgbd_sync:=true depth_topic:=/oakd/rgb/preview/depth odom_sensor_sync:=true camera_info_topic:=/oakd/rgb/preview/camera_info rgb_topic:=/oakd/rgb/preview/image_raw visual_odometry:=false approx_sync:=true approx_rgbd_sync:=false odom_guess_frame_id:=odom icp_odometry:=true odom_topic:="icp_odom" map_topic:="/map" qos:=2 use_sim_time:=true odom_log_level:=warn rtabmap_args:="--delete_db_on_start --Reg/Strategy 1 --Reg/Force3DoF true --Mem/NotLinkedNodesKept false" use_action_for_goal:=true
#
# 3) Click on "Play" button on bottom-left of gazebo.
#
# 4) Click on double points ".." button on top-right next to power button to undock.
#
# 5) Move the robot:
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
# a) By teleoperating:
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
# c) By using autonomous exploration node (tested with https://github.com/robo-friends/m-explore-ros2):
# $ ros2 launch explore_lite explore.launch.py
#
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
qos = LaunchConfiguration('qos')
localization = LaunchConfiguration('localization')
icp_parameters={
'odom_frame_id':'icp_odom',
'guess_frame_id':'odom',
'qos':qos
}
rtabmap_parameters={
'subscribe_rgbd':True,
'subscribe_scan':True,
'use_action_for_goal':True,
'odom_sensor_sync': True,
'qos_scan':qos,
'qos_image':qos,
'qos_imu':qos,
# RTAB-Map's parameters should be strings:
'Mem/NotLinkedNodesKept':'false'
}
# Shared parameters between different nodes
shared_parameters={
'frame_id':'base_link',
'use_sim_time':use_sim_time,
# RTAB-Map's parameters should be strings:
'Reg/Strategy':'1',
'Reg/Force3DoF':'true',
'Mem/NotLinkedNodesKept':'false',
'Icp/PointToPlaneMinComplexity':'0.04' # to be more robust to long corridors with low geometry
}
remappings=[
('odom', 'icp_odom'),
('rgb/image', '/oakd/rgb/preview/image_raw'),
('rgb/camera_info', '/oakd/rgb/preview/camera_info'),
('depth/image', '/oakd/rgb/preview/depth')]
return LaunchDescription([
# Launch arguments
DeclareLaunchArgument(
'use_sim_time', default_value='false', choices=['true', 'false'],
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument(
'qos', default_value='0',
description='QoS used for input sensor topics'),
DeclareLaunchArgument(
'localization', default_value='false', choices=['true', 'false'],
description='Launch rtabmap in localization mode (a map should have been already created).'),
# Nodes to launch
Node(
package='rtabmap_sync', executable='rgbd_sync', output='screen',
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time, 'qos':qos}],
remappings=remappings),
Node(
package='rtabmap_odom', executable='icp_odometry', output='screen',
parameters=[icp_parameters, shared_parameters],
remappings=remappings,
arguments=["--ros-args", "--log-level", 'icp_odometry:=warn']),
# SLAM Mode:
Node(
condition=UnlessCondition(localization),
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=[rtabmap_parameters, shared_parameters],
remappings=remappings,
arguments=['-d']),
# Localization mode:
Node(
condition=IfCondition(localization),
package='rtabmap_slam', executable='rtabmap', output='screen',
parameters=[rtabmap_parameters, shared_parameters,
{'Mem/IncrementalMemory':'False',
'Mem/InitWMWithAllNodes':'True'}],
remappings=remappings),
Node(
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
parameters=[rtabmap_parameters, shared_parameters],
remappings=remappings),
])
rtabmap_launch/launch/rtabmap.launch.py
+34 -14
View File
@@ -45,6 +45,7 @@ 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('sync_queue_size', default_value=LaunchConfiguration('queue_size'), description='Queue size of topic synchronizers.'),
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.'),
@@ -53,6 +54,8 @@ def launch_setup(context, *args, **kwargs):
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.'),
DeclareLaunchArgument('odom_log_level', default_value=LaunchConfiguration('log_level'), description='Specific ROS logger level for odometry node.'),
#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!'),
DeclareLaunchArgument('depth_topic_relay', default_value=ConditionalText(''.join([LaunchConfiguration('depth_topic').perform(context), "_relay"]), ''.join(LaunchConfiguration('depth_topic').perform(context)), LaunchConfiguration('compressed').perform(context)), description='Should not be modified manually!'),
@@ -86,7 +89,8 @@ def launch_setup(context, *args, **kwargs):
parameters=[{
"approx_sync": LaunchConfiguration('approx_rgbd_sync'),
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
"queue_size": LaunchConfiguration('queue_size'),
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"depth_scale": LaunchConfiguration('depth_scale')}],
@@ -120,7 +124,8 @@ def launch_setup(context, *args, **kwargs):
parameters=[{
"approx_sync": LaunchConfiguration('approx_rgbd_sync'),
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
"queue_size": LaunchConfiguration('queue_size'),
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info')}],
remappings=[
@@ -167,7 +172,8 @@ def launch_setup(context, *args, **kwargs):
"approx_sync": LaunchConfiguration('approx_sync'),
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
"config_path": LaunchConfiguration('cfg').perform(context),
"queue_size": LaunchConfiguration('queue_size'),
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"qos_imu": LaunchConfiguration('qos_imu'),
@@ -182,7 +188,7 @@ def launch_setup(context, *args, **kwargs):
("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
("odom", LaunchConfiguration('odom_topic')),
("imu", LaunchConfiguration('imu_topic'))],
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rgbd_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['rgbd_odometry:=', LaunchConfiguration('odom_log_level')]],
prefix=LaunchConfiguration('launch_prefix'),
namespace=LaunchConfiguration('namespace')),
@@ -201,7 +207,8 @@ def launch_setup(context, *args, **kwargs):
"approx_sync": LaunchConfiguration('approx_sync'),
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
"config_path": LaunchConfiguration('cfg').perform(context),
"queue_size": LaunchConfiguration('queue_size'),
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"qos_imu": LaunchConfiguration('qos_imu'),
@@ -217,7 +224,7 @@ def launch_setup(context, *args, **kwargs):
("rgbd_image", LaunchConfiguration('rgbd_topic_relay')),
("odom", LaunchConfiguration('odom_topic')),
("imu", LaunchConfiguration('imu_topic'))],
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.stereo_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['stereo_odometry:=', LaunchConfiguration('odom_log_level')]],
prefix=LaunchConfiguration('launch_prefix'),
namespace=LaunchConfiguration('namespace')),
@@ -235,7 +242,8 @@ def launch_setup(context, *args, **kwargs):
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg').perform(context),
"queue_size": LaunchConfiguration('queue_size'),
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
"qos": LaunchConfiguration('qos_image'),
"qos_imu": LaunchConfiguration('qos_imu'),
"guess_frame_id": LaunchConfiguration('odom_guess_frame_id').perform(context),
@@ -246,7 +254,7 @@ def launch_setup(context, *args, **kwargs):
("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
("odom", LaunchConfiguration('odom_topic')),
("imu", LaunchConfiguration('imu_topic'))],
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args")],
arguments=[LaunchConfiguration("args"), LaunchConfiguration("odom_args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.icp_odometry:=', LaunchConfiguration('odom_log_level')], "--log-level", ['icp_odometry:=', LaunchConfiguration('odom_log_level')]],
prefix=LaunchConfiguration('launch_prefix'),
namespace=LaunchConfiguration('namespace')),
@@ -266,6 +274,7 @@ def launch_setup(context, *args, **kwargs):
"odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context),
"publish_tf": LaunchConfiguration('publish_tf_map'),
"initial_pose": LaunchConfiguration('initial_pose'),
"use_action_for_goal": LaunchConfiguration('use_action_for_goal'),
"ground_truth_frame_id": LaunchConfiguration('ground_truth_frame_id').perform(context),
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
"odom_tf_angular_variance": LaunchConfiguration('odom_tf_angular_variance'),
@@ -275,7 +284,8 @@ def launch_setup(context, *args, **kwargs):
"database_path": LaunchConfiguration('database_path'),
"approx_sync": LaunchConfiguration('approx_sync'),
"config_path": LaunchConfiguration('cfg').perform(context),
"queue_size": LaunchConfiguration('queue_size'),
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
"qos_image": LaunchConfiguration('qos_image'),
"qos_scan": LaunchConfiguration('qos_scan'),
"qos_odom": LaunchConfiguration('qos_odom'),
@@ -290,6 +300,7 @@ def launch_setup(context, *args, **kwargs):
"Mem/InitWMWithAllNodes": ConditionalText("true", "false", IfCondition(PythonExpression(["'", LaunchConfiguration('localization'), "' == 'true'"]))._predicate_func(context)).perform(context)
}],
remappings=[
("map", LaunchConfiguration('map_topic')),
("rgb/image", LaunchConfiguration('rgb_topic_relay')),
("depth/image", LaunchConfiguration('depth_topic_relay')),
("rgb/camera_info", LaunchConfiguration('camera_info_topic')),
@@ -306,8 +317,9 @@ def launch_setup(context, *args, **kwargs):
("tag_detections", LaunchConfiguration('tag_topic')),
("fiducial_transforms", LaunchConfiguration('fiducial_topic')),
("odom", LaunchConfiguration('odom_topic')),
("imu", LaunchConfiguration('imu_topic'))],
arguments=[LaunchConfiguration("args")],
("imu", LaunchConfiguration('imu_topic')),
("goal_out", LaunchConfiguration('output_goal_topic'))],
arguments=[LaunchConfiguration("args"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap:=', LaunchConfiguration('log_level')]],
prefix=LaunchConfiguration('launch_prefix'),
namespace=LaunchConfiguration('namespace')),
@@ -326,7 +338,8 @@ def launch_setup(context, *args, **kwargs):
"odom_frame_id": LaunchConfiguration('odom_frame_id').perform(context),
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
"approx_sync": LaunchConfiguration('approx_sync'),
"queue_size": LaunchConfiguration('queue_size'),
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
"sync_queue_size": LaunchConfiguration('sync_queue_size'),
"qos_image": LaunchConfiguration('qos_image'),
"qos_scan": LaunchConfiguration('qos_scan'),
"qos_odom": LaunchConfiguration('qos_odom'),
@@ -346,7 +359,7 @@ def launch_setup(context, *args, **kwargs):
("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
("odom", LaunchConfiguration('odom_topic'))],
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
arguments=[LaunchConfiguration("gui_cfg")],
arguments=[LaunchConfiguration("gui_cfg"), "--ros-args", "--log-level", [LaunchConfiguration('namespace'), '.rtabmap_viz:=', LaunchConfiguration('log_level')], "--log-level", ['rtabmap_viz:=', LaunchConfiguration('log_level')]],
prefix=LaunchConfiguration('launch_prefix'),
namespace=LaunchConfiguration('namespace')),
Node(
@@ -391,6 +404,8 @@ def generate_launch_description():
DeclareLaunchArgument('use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument('log_level', default_value='info', description="ROS logging level (debug, info, warn, error). For RTAB-Map\'s logger level, use \"args\" argument."),
# Config files
DeclareLaunchArgument('cfg', default_value='', description='To change RTAB-Map\'s parameters, set the path of config file (*.ini) generated by the standalone app.'),
DeclareLaunchArgument('gui_cfg', default_value='~/.ros/rtabmap_gui.ini', description='Configuration path of rtabmap_viz.'),
@@ -399,10 +414,12 @@ def generate_launch_description():
DeclareLaunchArgument('frame_id', default_value='base_link', description='Fixed frame id of the robot (base frame), you may set "base_link" or "base_footprint" if they are published. For camera-only config, this could be "camera_link".'),
DeclareLaunchArgument('odom_frame_id', default_value='', description='If set, TF is used to get odometry instead of the topic.'),
DeclareLaunchArgument('map_frame_id', default_value='map', description='Output map frame id (TF).'),
DeclareLaunchArgument('map_topic', default_value='map', description='Map topic name.'),
DeclareLaunchArgument('publish_tf_map', default_value='true', description='Publish TF between map and odomerty.'),
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('topic_queue_size', default_value='1', description='Queue size of individual topic subscribers.'),
DeclareLaunchArgument('queue_size', default_value='10', description='Backward compatibility, use "sync_queue_size" instead.'),
DeclareLaunchArgument('qos', default_value='1', 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.'),
@@ -410,6 +427,9 @@ def generate_launch_description():
DeclareLaunchArgument('output', default_value='screen', description='Control node output (screen or log).'),
DeclareLaunchArgument('initial_pose', default_value='', description='Set an initial pose (only in localization mode). Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc'),
DeclareLaunchArgument('output_goal_topic', default_value='/goal_pose', description='Output goal topic (can be connected to nav2).'),
DeclareLaunchArgument('use_action_for_goal', default_value='false', description='Connect to nav2\'s navigate_to_pose action server instead of publishing the output goal topic.'),
DeclareLaunchArgument('ground_truth_frame_id', default_value='', description='e.g., "world"'),
DeclareLaunchArgument('ground_truth_base_frame_id', default_value='', description='e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree)'),
rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
+6 -1
View File
@@ -41,7 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap_msgs/msg/rgbd_images.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
namespace rtabmap_odom
{
@@ -144,7 +148,8 @@ private:
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int queueSize_;
int topicQueueSize_;
int syncQueueSize_;
bool keepColor_;
};
rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
+6 -1
View File
@@ -37,7 +37,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.hpp>
#include <image_transport/subscriber_filter.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <sensor_msgs/msg/image.hpp>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap_msgs/msg/rgbd_images.hpp>
@@ -142,7 +146,8 @@ private:
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int queueSize_;
int topicQueueSize_;
int syncQueueSize_;
bool keepColor_;
};
rtabmap_odom/src/OdometryROS.cpp
+4
View File
@@ -34,7 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl_conversions/pcl_conversions.h>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <rtabmap/core/odometry/OdometryF2M.h>
#include <rtabmap/core/odometry/OdometryF2F.h>
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+55 -39
View File
@@ -27,7 +27,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_odom/rgbd_odometry.hpp>
#ifdef PRE_ROS_IRON
#include <image_geometry/stereo_camera_model.h>
#else
#include <image_geometry/stereo_camera_model.hpp>
#endif
#include <sensor_msgs/image_encodings.hpp>
@@ -59,7 +63,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5),
topicQueueSize_(1),
syncQueueSize_(5),
keepColor_(false)
{
OdometryROS::init(false, true, false);
@@ -89,7 +94,17 @@ void RGBDOdometry::onOdomInit()
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize_ = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize_);
}
syncQueueSize_ = this->declare_parameter("sync_queue_size", syncQueueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
@@ -102,8 +117,9 @@ void RGBDOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
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: topic_queue_size = %d", topicQueueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: sync_queue_size = %d", syncQueueSize_);
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);
@@ -115,23 +131,23 @@ void RGBDOdometry::onOdomInit()
{
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras == 2)
@@ -139,7 +155,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
@@ -149,7 +165,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -166,7 +182,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -177,7 +193,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -196,7 +212,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -208,7 +224,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -229,7 +245,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -242,7 +258,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -265,7 +281,7 @@ void RGBDOdometry::onOdomInit()
if(approxSync)
{
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -279,7 +295,7 @@ void RGBDOdometry::onOdomInit()
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -311,7 +327,7 @@ void RGBDOdometry::onOdomInit()
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
subscribedTopic = rgbdxSub_->get_topic_name();
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
@@ -320,7 +336,7 @@ void RGBDOdometry::onOdomInit()
}
else
{
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopic = rgbdSub_->get_topic_name();
subscribedTopicsMsg =
@@ -332,20 +348,20 @@ void RGBDOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
@@ -737,20 +753,20 @@ void RGBDOdometry::flushCallbacks()
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(approxSync2_)
{
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -759,7 +775,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -768,7 +784,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -778,7 +794,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -788,7 +804,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync4_;
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -799,7 +815,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync4_;
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -810,7 +826,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -822,7 +838,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -834,7 +850,7 @@ void RGBDOdometry::flushCallbacks()
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -847,7 +863,7 @@ void RGBDOdometry::flushCallbacks()
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+57 -41
View File
@@ -29,7 +29,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/image_encodings.hpp>
#ifdef PRE_ROS_IRON
#include <image_geometry/stereo_camera_model.h>
#else
#include <image_geometry/stereo_camera_model.hpp>
#endif
#include "rtabmap_conversions/MsgConversion.h"
#include <rtabmap_msgs/msg/rgbd_images.hpp>
@@ -59,7 +63,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5),
topicQueueSize_(1),
syncQueueSize_(5),
keepColor_(false)
{
OdometryROS::init(true, true, false);
@@ -89,7 +94,17 @@ void StereoOdometry::onOdomInit()
int rgbdCameras = 1;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
topicQueueSize_ = this->declare_parameter("topic_queue_size", topicQueueSize_);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize_ = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize_);
}
syncQueueSize_ = this->declare_parameter("sync_queue_size", syncQueueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
@@ -98,9 +113,10 @@ void StereoOdometry::onOdomInit()
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
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: topic_queue_size = %d", topicQueueSize_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: sync_queue_size = %d", syncQueueSize_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "StereoOdometry: 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");
@@ -110,23 +126,23 @@ void StereoOdometry::onOdomInit()
{
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras == 2)
@@ -134,7 +150,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
@@ -144,7 +160,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -161,7 +177,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -172,7 +188,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -191,7 +207,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -203,7 +219,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -224,7 +240,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -237,7 +253,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -260,7 +276,7 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -274,7 +290,7 @@ void StereoOdometry::onOdomInit()
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -307,7 +323,7 @@ void StereoOdometry::onOdomInit()
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
subscribedTopic = rgbdxSub_->get_topic_name();
subscribedTopicsMsg =
@@ -317,7 +333,7 @@ void StereoOdometry::onOdomInit()
}
else
{
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopic = rgbdSub_->get_topic_name();
subscribedTopicsMsg =
@@ -329,21 +345,21 @@ void StereoOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
@@ -913,20 +929,20 @@ void StereoOdometry::flushCallbacks()
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
if(approxSync2_)
{
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -935,7 +951,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -944,7 +960,7 @@ void StereoOdometry::flushCallbacks()
{
delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -954,7 +970,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
@@ -964,7 +980,7 @@ void StereoOdometry::flushCallbacks()
{
delete approxSync4_;
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -975,7 +991,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync4_;
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -986,7 +1002,7 @@ void StereoOdometry::flushCallbacks()
{
delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -998,7 +1014,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -1010,7 +1026,7 @@ void StereoOdometry::flushCallbacks()
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
@@ -1023,7 +1039,7 @@ void StereoOdometry::flushCallbacks()
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rtabmap_rviz_plugins/CMakeLists.txt
+4 -22
View File
@@ -46,21 +46,6 @@ MESSAGE(STATUS "rtabmap_conversions=${rtabmap_conversions_LIBRARIES}")
## We also use Ogre for rviz plugins
include_directories( ${OGRE_INCLUDE_DIRS} )
## RVIZ plugin
IF(QT4_FOUND)
qt4_wrap_cpp(MOC_FILES
include/${PROJECT_NAME}/MapCloudDisplay.h
include/${PROJECT_NAME}/MapGraphDisplay.h
include/${PROJECT_NAME}/InfoDisplay.h
)
ELSE()
qt5_wrap_cpp(MOC_FILES
include/${PROJECT_NAME}/MapCloudDisplay.h
include/${PROJECT_NAME}/MapGraphDisplay.h
include/${PROJECT_NAME}/InfoDisplay.h
)
ENDIF()
# tf:message_filters, mixing boost and Qt signals
set_property(
SOURCE src/MapCloudDisplay.cpp src/MapGraphDisplay.cpp src/InfoDisplay.cpp src/OrbitOrientedViewController.cpp
@@ -71,8 +56,11 @@ add_library(rtabmap_rviz_plugins SHARED
src/MapCloudDisplay.cpp
src/MapGraphDisplay.cpp
src/InfoDisplay.cpp
${MOC_FILES}
include/${PROJECT_NAME}/MapCloudDisplay.h
include/${PROJECT_NAME}/MapGraphDisplay.h
include/${PROJECT_NAME}/InfoDisplay.h
)
set_property(TARGET rtabmap_rviz_plugins PROPERTY AUTOMOC ON)
target_include_directories(rtabmap_rviz_plugins
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -81,10 +69,6 @@ target_include_directories(rtabmap_rviz_plugins
ament_target_dependencies(rtabmap_rviz_plugins ${Libraries})
IF(Qt5_FOUND)
QT5_USE_MODULES(rtabmap_rviz_plugins Widgets Core Gui)
ENDIF(Qt5_FOUND)
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
target_compile_definitions(rtabmap_rviz_plugins PRIVATE "RTABMAP_ROS_BUILDING_LIBRARY")
@@ -111,6 +95,4 @@ install(TARGETS
INCLUDES DESTINATION include
)
pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml)
ament_package()
rtabmap_rviz_plugins/include/rtabmap_rviz_plugins/MapCloudDisplay.h
+1
View File
@@ -175,6 +175,7 @@ private:
void fillTransformerOptions( rviz_common::properties::EnumProperty* prop, uint32_t mask );
private:
std::shared_ptr<rclcpp::Node> clientNode_;
rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr republishNodeDataPub_;
std::map<int, CloudInfoPtr> cloud_infos_;
rtabmap_rviz_plugins/src/MapCloudDisplay.cpp
+47 -44
View File
@@ -57,7 +57,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap_msgs/srv/get_map.hpp>
namespace rtabmap_rviz_plugins
{
@@ -97,6 +96,10 @@ MapCloudDisplay::MapCloudDisplay()
//QIcon icon;
//this->setIcon(icon);
auto options = rclcpp::NodeOptions().arguments(
{"--ros-args", "--remap", "__node:=rviz_map_cloud_action_client", "--"});
clientNode_ = std::make_shared<rclcpp::Node>("_", options);
style_property_ = new rviz_common::properties::EnumProperty( "Style", "Flat Squares",
"Rendering mode to use, in order of computational complexity.",
this, SLOT( updateStyle() ), this );
@@ -500,64 +503,64 @@ void MapCloudDisplay::updateCloudParameters()
fromScan_ = cloud_from_scan_->getBool();
}
void MapCloudDisplay::downloadMap(bool /*graphOnly*/)
void MapCloudDisplay::downloadMap(bool graphOnly)
{
RCLCPP_ERROR(rviz_ros_node_.lock()->get_raw_node()->get_logger(), "MapCloud plugin: DownloadMap still not working on ros2");
return;
// FIXME: ros2: can connect to client, rtabmap returns data but the callback here is never called?!
/*
auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
request->global_map = false;
request->optimized = true;
request->graph_only = graphOnly;
std::string rtabmapNs = download_namespace->getStdString();
std::string srvName = uFormat("%s/get_map_data", rtabmapNs.c_str());
// QMessageBox * messageBox = new QMessageBox(
// QMessageBox::NoIcon,
// tr("Calling \"%1\" service...").arg(srvName.c_str()),
// tr("Downloading the map... please wait (rviz could become gray!)"),
// QMessageBox::NoButton);
// messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
// messageBox->show();
// QApplication::processEvents();
// uSleep(100); // hack make sure the text in the QMessageBox is shown...
// QApplication::processEvents();
std::string srvName = rtabmapNs+"/get_map_data";
QMessageBox * messageBox = new QMessageBox(
QMessageBox::NoIcon,
tr("Calling \"%1\" service...").arg(srvName.c_str()),
tr("Downloading the map... please wait (rviz could become gray!)"),
QMessageBox::NoButton,
getAssociatedWidget());
messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
messageBox->show();
QApplication::processEvents();
uSleep(100); // hack make sure the text in the QMessageBox is shown...
QApplication::processEvents();
RVIZ_COMMON_LOG_WARNING(uFormat("Wait for service %s", srvName.c_str()));
auto client = rviz_ros_node_.lock()->get_raw_node()->create_client<rtabmap_ros::srv::GetMap>(srvName);
RVIZ_COMMON_LOG_INFO(uFormat("Wait for service %s", srvName.c_str()));
auto client = clientNode_->create_client<rtabmap_msgs::srv::GetMap>(srvName);
if(client->wait_for_service(std::chrono::seconds(1)))
{
using ServiceResponseFuture = rclcpp::Client<rtabmap_ros::srv::GetMap>::SharedFuture;
auto response_received_callback = [this, &graphOnly](ServiceResponseFuture future) {
auto result = future.get();
RVIZ_COMMON_LOG_WARNING(uFormat("Process data"));
RVIZ_COMMON_LOG_INFO(uFormat("Calling service %s", srvName.c_str()));
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(clientNode_, result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
RVIZ_COMMON_LOG_INFO(uFormat("Process data"));
auto future = result.get();
if(graphOnly)
{
//messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(result->data.graph.poses.size()));
//QApplication::processEvents();
processMapData(result->data);
//messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(result->data.graph.poses.size()));
// QTimer::singleShot(1000, messageBox, SLOT(close()));
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(future->data.graph.poses.size()));
QApplication::processEvents();
processMapData(future->data);
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(future->data.graph.poses.size()));
QApplication::processEvents();
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
else
{
//messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
// .arg(result->data.graph.poses.size()).arg(result->data.nodes.size()));
//QApplication::processEvents();
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
QApplication::processEvents();
this->reset();
processMapData(result->data);
//messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
// .arg(result->data.graph.poses.size()).arg(result->data.nodes.size()));
processMapData(future->data);
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
.arg(future->data.graph.poses.size()).arg(future->data.nodes.size()));
// QTimer::singleShot(1000, messageBox, SLOT(close()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
};
RVIZ_COMMON_LOG_WARNING(uFormat("Calling service %s", srvName.c_str()));
auto result_future = client->async_send_request(request, response_received_callback);
RVIZ_COMMON_LOG_WARNING(uFormat("Wait"));
result_future.wait();
RVIZ_COMMON_LOG_WARNING(uFormat("Wait end"));
} else {
std::string msg = uFormat("Failed to call service %s", srvName.c_str());
RVIZ_COMMON_LOG_ERROR(msg);
messageBox->setText(msg.c_str());
}
}
else
{
@@ -567,8 +570,8 @@ void MapCloudDisplay::downloadMap(bool /*graphOnly*/)
srvName.c_str(),
rtabmapNs.c_str());
RVIZ_COMMON_LOG_ERROR(msg);
//messageBox->setText(msg.c_str());
}*/
messageBox->setText(msg.c_str());
}
}
void MapCloudDisplay::downloadNamespaceChanged()
rtabmap_slam/CMakeLists.txt
+15 -7
View File
@@ -14,7 +14,6 @@ find_package(ament_cmake REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
@@ -28,12 +27,9 @@ find_package(rtabmap_msgs REQUIRED)
find_package(rtabmap_util REQUIRED)
find_package(rtabmap_sync REQUIRED)
IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
ENDIF()
#optional
find_package(apriltag_msgs)
find_package(nav2_msgs)
IF(WIN32)
add_compile_options(-bigobj)
@@ -48,7 +44,6 @@ SET(Libraries
cv_bridge
geometry_msgs
nav_msgs
nav2_msgs
rclcpp
rclcpp_components
sensor_msgs
@@ -80,6 +75,19 @@ SET(Libraries
)
ENDIF(apriltag_msgs_FOUND)
# If nav2_msgs is found, add definition
IF(nav2_msgs_FOUND)
MESSAGE(STATUS "WITH nav2_msgs")
ADD_DEFINITIONS("-DWITH_NAV2_MSGS")
SET(Libraries
${Libraries}
nav2_msgs
)
IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
ADD_DEFINITIONS("-DNAV_MSGS_FOXY")
ENDIF()
ENDIF(nav2_msgs_FOUND)
############################
## Declare a cpp library
############################
@@ -128,4 +136,4 @@ install(DIRECTORY include/
FILES_MATCHING PATTERN "*.h"
)
ament_package()
ament_package()
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+8
View File
@@ -88,8 +88,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
#endif
#ifdef WITH_NAV2_MSGS
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#endif
//#define WITH_FIDUCIAL_MSGS
#ifdef WITH_FIDUCIAL_MSGS
@@ -109,8 +111,10 @@ public:
explicit CoreWrapper(const rclcpp::NodeOptions & options);
virtual ~CoreWrapper();
#ifdef WITH_NAV2_MSGS
using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNav2 = rclcpp_action::ClientGoalHandle<NavigateToPose>;
#endif
private:
bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
@@ -246,12 +250,14 @@ private:
void publishStats(const rclcpp::Time & stamp);
void publishCurrentGoal(const rclcpp::Time & stamp);
#ifdef WITH_NAV2_MSGS
#ifdef NAV_MSGS_FOXY
void goalResponseCallback(std::shared_future<GoalHandleNav2::SharedPtr> future);
#else
void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle);
#endif
void resultCallback(const GoalHandleNav2::WrappedResult & result);
#endif
void publishLocalPath(const rclcpp::Time & stamp);
void publishGlobalPath(const rclcpp::Time & stamp);
@@ -373,7 +379,9 @@ private:
rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapBinarySrv_;
rclcpp::Service<octomap_msgs::srv::GetOctomap>::SharedPtr octomapFullSrv_;
#endif
#ifdef WITH_NAV2_MSGS
rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
#endif
std::thread* transformThread_;
bool tfThreadRunning_;
rtabmap_slam/src/CoreWrapper.cpp
+65 -41
View File
@@ -36,9 +36,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_msgs/msg/bool.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <sensor_msgs/image_encodings.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <pcl_conversions/pcl_conversions.h>
#if PCL_VERSION_COMPARE(>, 1, 12, 0)
#include <pcl/common/io.h>
#else
#include <pcl/io/io.h>
#endif
#include <visualization_msgs/msg/marker_array.hpp>
@@ -196,6 +204,13 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr);
useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_);
#ifndef WITH_NAV2_MSGS
if(useActionForGoal_)
{
RCLCPP_ERROR(this->get_logger(), "rtabmap: Cannot enable use_action_for_goal because rtabmap_slam is not built with nav2_msgs support.");
useActionForGoal_ = false;
}
#endif
useSavedMap_ = this->declare_parameter("use_saved_map", useSavedMap_);
genScan_ = this->declare_parameter("gen_scan", genScan_);
genScanMaxDepth_ = this->declare_parameter("gen_scan_max_depth", genScanMaxDepth_);
@@ -633,45 +648,46 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
}
// setup services
updateSrv_ = this->create_service<std_srvs::srv::Empty>("update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resetSrv_ = this->create_service<std_srvs::srv::Empty>("reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
pauseSrv_ = this->create_service<std_srvs::srv::Empty>("pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resumeSrv_ = this->create_service<std_srvs::srv::Empty>("resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
loadDatabaseSrv_ = this->create_service<rtabmap_msgs::srv::LoadDatabase>("load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
triggerNewMapSrv_ = this->create_service<std_srvs::srv::Empty>("trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
backupDatabase_ = this->create_service<std_srvs::srv::Empty>("backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
detectMoreLoopClosuresSrv_ = this->create_service<rtabmap_msgs::srv::DetectMoreLoopClosures>("detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
globalBundleAdjustmentSrv_ = this->create_service<rtabmap_msgs::srv::GlobalBundleAdjustment>("global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
cleanupLocalGridsSrv_ = this->create_service<rtabmap_msgs::srv::CleanupLocalGrids>("cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setModeLocalizationSrv_ = this->create_service<std_srvs::srv::Empty>("set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setModeMappingSrv_ = this->create_service<std_srvs::srv::Empty>("set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getNodeDataSrv_ = this->create_service<rtabmap_msgs::srv::GetNodeData>("get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getMapDataSrv_ = this->create_service<rtabmap_msgs::srv::GetMap>("get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getMapData2Srv_ = this->create_service<rtabmap_msgs::srv::GetMap2>("get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getMapSrv_ = this->create_service<nav_msgs::srv::GetMap>("get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getProbMapSrv_ = this->create_service<nav_msgs::srv::GetMap>("get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
publishMapDataSrv_ = this->create_service<rtabmap_msgs::srv::PublishMap>("publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getPlanSrv_ = this->create_service<nav_msgs::srv::GetPlan>("get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getPlanNodesSrv_ = this->create_service<rtabmap_msgs::srv::GetPlan>("get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setGoalSrv_ = this->create_service<rtabmap_msgs::srv::SetGoal>("set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>("cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLabelSrv_ = this->create_service<rtabmap_msgs::srv::SetLabel>("set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
listLabelsSrv_ = this->create_service<rtabmap_msgs::srv::ListLabels>("list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
removeLabelSrv_ = this->create_service<rtabmap_msgs::srv::RemoveLabel>("remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
addLinkSrv_ = this->create_service<rtabmap_msgs::srv::AddLink>("add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getNodesInRadiusSrv_ = this->create_service<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
const std::string servicePrefix = get_name() + std::string("/");
updateSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resetSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
loadDatabaseSrv_ = this->create_service<rtabmap_msgs::srv::LoadDatabase>(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
triggerNewMapSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
backupDatabase_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
detectMoreLoopClosuresSrv_ = this->create_service<rtabmap_msgs::srv::DetectMoreLoopClosures>(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
globalBundleAdjustmentSrv_ = this->create_service<rtabmap_msgs::srv::GlobalBundleAdjustment>(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
cleanupLocalGridsSrv_ = this->create_service<rtabmap_msgs::srv::CleanupLocalGrids>(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setModeLocalizationSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setModeMappingSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getNodeDataSrv_ = this->create_service<rtabmap_msgs::srv::GetNodeData>(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getMapDataSrv_ = this->create_service<rtabmap_msgs::srv::GetMap>(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getMapData2Srv_ = this->create_service<rtabmap_msgs::srv::GetMap2>(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getProbMapSrv_ = this->create_service<nav_msgs::srv::GetMap>(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
publishMapDataSrv_ = this->create_service<rtabmap_msgs::srv::PublishMap>(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getPlanSrv_ = this->create_service<nav_msgs::srv::GetPlan>(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getPlanNodesSrv_ = this->create_service<rtabmap_msgs::srv::GetPlan>(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setGoalSrv_ = this->create_service<rtabmap_msgs::srv::SetGoal>(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
cancelGoalSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLabelSrv_ = this->create_service<rtabmap_msgs::srv::SetLabel>(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
listLabelsSrv_ = this->create_service<rtabmap_msgs::srv::ListLabels>(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
removeLabelSrv_ = this->create_service<rtabmap_msgs::srv::RemoveLabel>(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
addLinkSrv_ = this->create_service<rtabmap_msgs::srv::AddLink>(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
getNodesInRadiusSrv_ = this->create_service<rtabmap_msgs::srv::GetNodesInRadius>(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>("octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>("octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
octomapBinarySrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
octomapFullSrv_ = this->create_service<octomap_msgs::srv::GetOctomap>(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
#endif
#endif
//private services
setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>("log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>("log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>("log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>("log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogDebugSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogInfoSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogWarnSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
setLogErrorSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
int optimizeIterations = 0;
Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
@@ -732,7 +748,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(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile());
defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).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());
@@ -834,7 +850,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
fiducialTransfromsSub_ = this->create_subscription<fiducial_msgs::msg::FiducialTransformArray>("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1));
#endif
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));
republishNodeDataSub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(servicePrefix+"republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1));
parametersClient_ = std::make_shared<rclcpp::SyncParametersClient>(this);
auto on_parameter_event_callback =
@@ -2199,7 +2215,11 @@ void CoreWrapper::process(
{
// Don't send status yet if nav2 actionlib is used unless it failed,
// let nav2 finish reaching the goal
#ifdef WITH_NAV2_MSGS
if(nav2Client_ == 0 || rtabmap_.getPathStatus() <= 0)
#else
if(rtabmap_.getPathStatus() <= 0)
#endif
{
if(rtabmap_.getPathStatus() > 0)
{
@@ -2209,10 +2229,12 @@ void CoreWrapper::process(
else if(rtabmap_.getPathStatus() <= 0)
{
RCLCPP_WARN(this->get_logger(), "Planning: Plan failed!");
#ifdef WITH_NAV2_MSGS
if(nav2Client_.get()!=NULL && nav2Client_->action_server_is_ready())
{
nav2Client_->async_cancel_all_goals();
}
#endif
}
if(goalReachedPub_->get_subscription_count())
@@ -3964,11 +3986,12 @@ void CoreWrapper::cancelGoalCallback(
goalReachedPub_->publish(result);
}
}
#ifdef WITH_NAV2_MSGS
if(nav2Client_.get() != NULL && nav2Client_->action_server_is_ready())
{
nav2Client_->async_cancel_all_goals();
}
#endif
}
void CoreWrapper::setLabelCallback(
@@ -4364,6 +4387,7 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
poseMsg.header.frame_id = mapFrameId_;
poseMsg.header.stamp = stamp;
rtabmap_conversions::transformToPoseMsg(currentMetricGoal_, poseMsg.pose);
#ifdef WITH_NAV2_MSGS
if(useActionForGoal_)
{
if(nav2Client_.get() == NULL || !nav2Client_->action_server_is_ready())
@@ -4395,17 +4419,16 @@ void CoreWrapper::publishCurrentGoal(const rclcpp::Time & stamp)
RCLCPP_ERROR(this->get_logger(), "Cannot connect to navigate_to_pose action server!");
}
}
else
#endif
if(nextMetricGoalPub_->get_subscription_count())
{
nextMetricGoalPub_->publish(poseMsg);
if(!useActionForGoal_)
{
lastPublishedMetricGoal_ = currentMetricGoal_;
}
lastPublishedMetricGoal_ = currentMetricGoal_;
}
}
}
#ifdef WITH_NAV2_MSGS
void CoreWrapper::goalResponseCallback(
#ifdef NAV_MSGS_FOXY
std::shared_future<GoalHandleNav2::SharedPtr> future)
@@ -4473,6 +4496,7 @@ void CoreWrapper::resultCallback(
latestNodeWasReached_ = false;
}
}
#endif
void CoreWrapper::publishLocalPath(const rclcpp::Time & stamp)
{
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+21 -41
View File
@@ -37,7 +37,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.hpp>
#include <image_transport/subscriber_filter.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/image.hpp>
@@ -74,7 +78,8 @@ public:
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
int getQueueSize() const {return queueSize_;}
int getTopicQueueSize() const {return topicQueueSize_;}
int getSyncQueueSize() const {return syncQueueSize_;}
bool isApproxSync() const {return approxSync_;}
const std::string & name() const {return name_;}
@@ -137,15 +142,11 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupStereoCallbacks(
rclcpp::Node & node,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupRGBCallbacks(
rclcpp::Node & node,
bool subscribeOdom,
@@ -153,9 +154,7 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupRGBDCallbacks(
rclcpp::Node & node,
bool subscribeOdom,
@@ -163,9 +162,7 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupRGBDXCallbacks(
rclcpp::Node & node,
bool subscribeOdom,
@@ -173,9 +170,7 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
#ifdef RTABMAP_SYNC_MULTI_RGBD
void setupRGBD2Callbacks(
rclcpp::Node & node,
@@ -184,9 +179,7 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupRGBD3Callbacks(
rclcpp::Node & node,
bool subscribeOdom,
@@ -194,9 +187,7 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupRGBD4Callbacks(
rclcpp::Node & node,
bool subscribeOdom,
@@ -204,9 +195,7 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupRGBD5Callbacks(
rclcpp::Node & node,
bool subscribeOdom,
@@ -214,9 +203,7 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupRGBD6Callbacks(
rclcpp::Node & node,
bool subscribeOdom,
@@ -224,35 +211,28 @@ private:
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
#endif
void setupSensorDataCallbacks(
rclcpp::Node & node,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupScanCallbacks(
rclcpp::Node & node,
bool subscribeScan2d,
bool subscribeScanDesc,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
void setupOdomCallbacks(
rclcpp::Node & node,
bool subscribeUserData,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
bool subscribeOdomInfo);
protected:
std::string subscribedTopicsMsg_;
int queueSize_;
int topicQueueSize_;
int syncQueueSize_;
rmw_qos_reliability_policy_t qosOdom_;
rmw_qos_reliability_policy_t qosImage_;
rmw_qos_reliability_policy_t qosCameraInfo_;
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h
+1 -1
View File
@@ -181,7 +181,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
} \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \
approxSync?"approx":"exact", \
APPROX?"approx":"exact", \
getTopicName(SUB0.getSubscriber()).c_str(), \
getTopicName(SUB1.getSubscriber()).c_str(), \
getTopicName(SUB2.getSubscriber()).c_str(), \
rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
+8 -8
View File
@@ -16,14 +16,14 @@ namespace rtabmap_sync {
class SyncDiagnostic {
public:
SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.1, int windowSize = 5) :
node_(node),
diagnosticUpdater_(node),
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
timeStampStatus_(diagnostic_updater::TimeStampStatusParam()),
compositeTask_("Sync status"),
lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
targetFrequency_(0.0),
windowSize_(windowSize)
node_(node),
diagnosticUpdater_(node),
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance), node->get_clock()),
timeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
compositeTask_("Sync status"),
lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
targetFrequency_(0.0),
windowSize_(windowSize)
{
UASSERT(windowSize_ >= 1);
}
rtabmap_sync/src/CommonDataSubscriber.cpp
+29 -43
View File
@@ -31,7 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync {
CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
queueSize_(10),
topicQueueSize_(1),
syncQueueSize_(10),
approxSync_(true),
subscribedToDepth_(!gui),
subscribedToStereo_(false),
@@ -378,7 +379,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_);
topicQueueSize_ = node.declare_parameter("topic_queue_size", topicQueueSize_);
int queueSize = node.declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize_ = queueSize;
RCLCPP_WARN(node.get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize_);
}
syncQueueSize_ = node.declare_parameter("sync_queue_size", syncQueueSize_);
int qos = node.declare_parameter("qos", 0);
int qosOdom = node.declare_parameter("qos_odom", qos);
@@ -519,7 +530,8 @@ void CommonDataSubscriber::setupCallbacks(
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: topic_queue_size = %d", name_.c_str(), topicQueueSize_);
RCLCPP_INFO(node.get_logger(), "%s: sync_queue_size = %d", name_.c_str(), syncQueueSize_);
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_);
@@ -537,18 +549,14 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToStereo_)
{
setupStereoCallbacks(
node,
subscribedToOdom_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToRGB_)
{
@@ -559,9 +567,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToRGBD_)
{
@@ -582,9 +588,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(rgbdCameras_ == 5)
{
@@ -595,9 +599,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(rgbdCameras_ == 4)
{
@@ -608,9 +610,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(rgbdCameras_ == 3)
{
@@ -621,9 +621,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(rgbdCameras_ == 2)
{
@@ -634,9 +632,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
#else
if(rgbdCameras_>1)
@@ -656,9 +652,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else
{
@@ -669,9 +663,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
}
else if(subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_)
@@ -682,27 +674,21 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScanDescriptor_,
subscribedToOdom_,
subscribedToUserData_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToSensorData_)
{
setupSensorDataCallbacks(
node,
subscribedToOdom_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToOdom_)
{
setupOdomCallbacks(
node,
subscribedToUserData_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
@@ -716,7 +702,7 @@ void CommonDataSubscriber::setupCallbacks(
"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
name_.c_str(),
approxSync_?
uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
uFormat("If topics are not published at the same rate, you could increase \"sync_queue_size\" and/or \"topic_queue_size\" parameters (current=%d and %d respectively).", syncQueueSize_, topicQueueSize_).c_str():
"Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg_.c_str()),
otherTasks);
rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp
+69 -71
View File
@@ -466,202 +466,200 @@ void CommonDataSubscriber::setupDepthCallbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup depth callback");
image_transport::TransportHints hints(&node);
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile());
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, depthData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
#endif
@@ -670,57 +668,57 @@ void CommonDataSubscriber::setupDepthCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, depth, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp
+9 -11
View File
@@ -66,29 +66,27 @@ void CommonDataSubscriber::odomDataInfoCallback(
void CommonDataSubscriber::setupOdomCallbacks(
rclcpp::Node& node,
bool subscribeUserData,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup scan callback");
if(subscribeUserData || subscribeOdomInfo)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_);
SYNC_DECL2(CommonDataSubscriber, odomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_);
}
}
else
@@ -96,13 +94,13 @@ void CommonDataSubscriber::setupOdomCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_);
}
}
else
{
odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::QoS(1).reliability(qosOdom_), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp
+67 -69
View File
@@ -466,201 +466,199 @@ void CommonDataSubscriber::setupRGBCallbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback");
image_transport::TransportHints hints(&node);
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile());
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_);
}
}
#endif
@@ -669,57 +667,57 @@ void CommonDataSubscriber::setupRGBCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
SYNC_DECL2(CommonDataSubscriber, rgb, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_);
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp
+42 -45
View File
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_sync {
@@ -541,9 +540,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd callback");
@@ -558,152 +555,152 @@ void CommonDataSubscriber::setupRGBDCallbacks(
{
rgbdSubs_.resize(1);
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]));
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]));
}
}
#endif
@@ -712,41 +709,41 @@ void CommonDataSubscriber::setupRGBDCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
@@ -756,7 +753,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
}
else
{
rgbdSub_ = node.create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
rgbdSub_ = node.create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp
+42 -45
View File
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_sync {
@@ -353,9 +352,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd2 callback");
@@ -363,152 +360,152 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
for(int i=0; i<2; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
#endif
@@ -517,45 +514,45 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp
+43 -46
View File
@@ -28,8 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_sync/CommonDataSubscriber.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <cv_bridge/cv_bridge.h>
#include "../../../rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h"
#include <rtabmap_conversions/MsgConversion.h>
namespace rtabmap_sync {
@@ -441,9 +440,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDescriptor,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd3 callback");
@@ -451,152 +448,152 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
for(int i=0; i<3; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
#endif
@@ -605,46 +602,46 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp
+42 -45
View File
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_sync {
@@ -410,9 +409,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd4 callback");
@@ -420,152 +417,152 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
for(int i=0; i<4; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
#endif
@@ -574,45 +571,45 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp
+21 -24
View File
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_sync {
@@ -262,9 +261,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd5 callback");
@@ -272,53 +269,53 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
for(int i=0; i<5; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
}
}
else
@@ -326,45 +323,45 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp
+21 -24
View File
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_sync {
@@ -280,9 +279,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd6 callback");
@@ -290,53 +287,53 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
for(int i=0; i<6; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).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_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
{
SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
}
}
else
@@ -344,45 +341,45 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).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_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp
+42 -45
View File
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_sync {
@@ -329,157 +328,155 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbdX callback");
rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_);
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync, queueSize, odomSub_, rgbdXSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync, queueSize, odomSub_, rgbdXSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync, queueSize, odomSub_, rgbdXSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync, queueSize, odomSub_, rgbdXSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync, queueSize, userDataSub_, rgbdXSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync, queueSize, userDataSub_, rgbdXSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync, queueSize, userDataSub_, rgbdXSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_);
}
}
#endif
@@ -488,46 +485,46 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync, queueSize, rgbdXSub_, scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync_, syncQueueSize_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync, queueSize, rgbdXSub_, scanSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync_, syncQueueSize_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync, queueSize, rgbdXSub_, scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync_, syncQueueSize_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_);
}
else
{
rgbdXSub_.unsubscribe();
rgbdXSubOnly_ = node.create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
rgbdXSubOnly_ = node.create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp
+42 -44
View File
@@ -253,9 +253,7 @@ void CommonDataSubscriber::setupScanCallbacks(
#else
bool,
#endif
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup scan callback");
@@ -268,36 +266,36 @@ void CommonDataSubscriber::setupScanCallbacks(
if(scanDescTopic)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
}
else if(scan2dTopic)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
}
else
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -305,12 +303,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_);
}
}
else
@@ -318,12 +316,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_);
}
}
}
@@ -331,19 +329,19 @@ void CommonDataSubscriber::setupScanCallbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync_, syncQueueSize_, odomSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -351,12 +349,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync_, syncQueueSize_, odomSub_, scanSub_);
}
}
else
@@ -364,31 +362,31 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync_, syncQueueSize_, odomSub_, scan3dSub_);
}
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -396,12 +394,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync_, syncQueueSize_, userDataSub_, scanSub_);
}
}
else
@@ -409,12 +407,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_);
}
}
}
@@ -422,18 +420,18 @@ void CommonDataSubscriber::setupScanCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(scanDescTopic)
{
SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync_, syncQueueSize_, scanDescSub_, odomInfoSub_);
}
else if(scan2dTopic)
{
SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync_, syncQueueSize_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync_, syncQueueSize_, scan3dSub_, odomInfoSub_);
}
}
}
@@ -442,7 +440,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(scanDescTopic)
{
subscribedToScanDescriptor_ = true;
scanDescSubOnly_ = node.create_subscription<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1));
scanDescSubOnly_ = node.create_subscription<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
@@ -451,7 +449,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else if(scan2dTopic)
{
subscribedToScan2d_ = true;
scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
@@ -460,7 +458,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else
{
subscribedToScan3d_ = true;
scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp
+9 -13
View File
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
namespace rtabmap_sync {
@@ -67,26 +66,23 @@ void CommonDataSubscriber::sensorDataOdomInfoCallback(
void CommonDataSubscriber::setupSensorDataCallbacks(
rclcpp::Node& node,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup SensorData callback");
sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(1).reliability(qosSensorData_).get_rmw_qos_profile());
sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_).get_rmw_qos_profile());
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync, queueSize, odomSub_, sensorDataSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync, queueSize, odomSub_, sensorDataSub_);
SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_);
}
}
else
@@ -94,13 +90,13 @@ void CommonDataSubscriber::setupSensorDataCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_);
}
else
{
sensorDataSub_.unsubscribe();
sensorDataSubOnly_ = node.create_subscription<rtabmap_msgs::msg::SensorData>("sensor_data", rclcpp::QoS(1).reliability(qosSensorData_), std::bind(&CommonDataSubscriber::sensorDataCallback, this, std::placeholders::_1));
sensorDataSubOnly_ = node.create_subscription<rtabmap_msgs::msg::SensorData>("sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_), std::bind(&CommonDataSubscriber::sensorDataCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp
+12 -14
View File
@@ -88,31 +88,29 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
void CommonDataSubscriber::setupStereoCallbacks(
rclcpp::Node& node,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup stereo callback");
image_transport::TransportHints hints(&node);
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile());
cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile());
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
}
}
else
@@ -120,12 +118,12 @@ void CommonDataSubscriber::setupStereoCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
SYNC_DECL4(CommonDataSubscriber, stereo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
}
}
}
rtabmap_sync/src/nodelets/rgb_sync.cpp
+24 -8
View File
@@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/image_encodings.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/Compression.h"
@@ -47,13 +51,24 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
approxSync_(0),
exactSync_(0)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
int qos = 0;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCaminfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
@@ -61,8 +76,9 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
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: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
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_);
@@ -71,20 +87,20 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_);
exactSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
}
image_transport::TransportHints hints(this);
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());
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
get_name(),
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+25 -9
View File
@@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/image_encodings.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/Compression.h"
@@ -49,13 +53,24 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
approxSyncDepth_(0),
exactSyncDepth_(0)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
depthScale_ = this->declare_parameter("depth_scale", depthScale_);
@@ -70,8 +85,9 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
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: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
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_);
@@ -82,21 +98,21 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/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());
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
get_name(),
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+23 -11
View File
@@ -42,21 +42,33 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
SYNC_INIT(rgbd7),
SYNC_INIT(rgbd8)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
int rgbdCameras = 2;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
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");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: rgbd_cameras = %d", get_name(), rgbdCameras);
@@ -68,14 +80,14 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
for(int i=0; i<rgbdCameras; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
}
std::string name_ = get_name();
std::string subscribedTopicsMsg_;
if(rgbdCameras==2)
{
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd2ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -83,7 +95,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==3)
{
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd3ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -91,7 +103,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==4)
{
SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL4(RGBDXSync, rgbd4, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd4ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -99,7 +111,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==5)
{
SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
SYNC_DECL5(RGBDXSync, rgbd5, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd5ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -107,7 +119,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==6)
{
SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
SYNC_DECL6(RGBDXSync, rgbd6, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd6ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -115,7 +127,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==7)
{
SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
SYNC_DECL7(RGBDXSync, rgbd7, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd7ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -123,7 +135,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==8)
{
SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
SYNC_DECL8(RGBDXSync, rgbd8, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd8ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
rtabmap_sync/src/nodelets/stereo_sync.cpp
+26 -10
View File
@@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/image_encodings.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/Compression.h"
@@ -46,21 +50,33 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
approxSync_(0),
exactSync_(0)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
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: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
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: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
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_);
@@ -69,22 +85,22 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
exactSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
image_transport::TransportHints hints(this);
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
get_name(),
rtabmap_util/scripts/yaml_to_camera_info.py
+3
View File
@@ -8,6 +8,9 @@ from sensor_msgs.msg import Image
def yaml_to_CameraInfo(yaml_fname):
with open(yaml_fname, "r") as file_handle:
first_line = file_handle.readline()
if "%YAML:" not in first_line:
file_handle.seek(0)
calib_data = yaml.load(file_handle, Loader=yaml.FullLoader)
msg = CameraInfo()
rtabmap_util/src/DbPlayerNode.cpp
+4
View File
@@ -36,7 +36,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rosgraph_msgs/Clock.h>
#include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/Odometry.h>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <image_transport/image_transport.h>
#include <tf2_ros/transform_broadcaster.h>
#include <std_srvs/Empty.h>
rtabmap_util/src/nodelets/disparity_to_depth.cpp
+4
View File
@@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
namespace rtabmap_util
{
rtabmap_util/src/nodelets/imu_to_tf.cpp
+1 -1
View File
@@ -27,7 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_util/imu_to_tf.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2/LinearMath/Transform.h>
namespace rtabmap_util
rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
+24 -13
View File
@@ -59,12 +59,23 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int queueSize = 5;
int topicQueueSize = 1;
int syncQueueSize = 5;
int count = 2;
bool approx=true;
double approxSyncMaxInterval = 0.0;
int qos=0;
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
frameId_ = this->declare_parameter("frame_id", frameId_);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
@@ -76,24 +87,24 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cloudSub_1_.subscribe(this, "cloud1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_2_.subscribe(this, "cloud2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_1_.subscribe(this, "cloud1", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_2_.subscribe(this, "cloud2", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
std::string subscribedTopicsMsg;
if(count == 4)
{
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_4_.subscribe(this, "cloud4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_4_.subscribe(this, "cloud4", rclcpp::QoS(topicQueueSize).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_);
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
exactSync4_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
@@ -107,17 +118,17 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
}
else if(count == 3)
{
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cloudSub_3_.subscribe(this, "cloud3", rclcpp::QoS(topicQueueSize).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_);
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
exactSync3_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
@@ -132,14 +143,14 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
{
if(approx)
{
approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_);
exactSync2_->registerCallback(std::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
rtabmap_util/src/nodelets/point_cloud_assembler.cpp
+23 -11
View File
@@ -73,11 +73,22 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int queueSize = 5;
int topicQueueSize = 1;
int syncQueueSize = 5;
int qos = 0;
bool subscribeOdomInfo = false;
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosOdom = this->declare_parameter("qos_odom", qos);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
@@ -97,7 +108,8 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
removeZ_ = this->declare_parameter("remove_z", removeZ_);
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: topic_queue_size=%d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size=%d", get_name(), syncQueueSize);
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());
@@ -128,17 +140,17 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
if(!fixedFrameId_.empty())
{
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(topicQueueSize).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", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
syncOdomInfoSub_.subscribe(this, "odom_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
syncOdomInfoSub_.subscribe(this, "odom_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
exactInfoSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
get_name(),
@@ -148,9 +160,9 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
}
else
{
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
syncCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
syncOdomSub_.subscribe(this, "odom", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosOdom).get_rmw_qos_profile());
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_);
exactSync_->registerCallback(std::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
get_name(),
rtabmap_util/src/nodelets/point_cloud_xyz.cpp
+28 -13
View File
@@ -30,10 +30,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_conversions/MsgConversion.h>
#include <pcl_conversions/pcl_conversions.h>
#include <image_geometry/pinhole_camera_model.h>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#include <image_geometry/pinhole_camera_model.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#include <image_geometry/pinhole_camera_model.hpp>
#endif
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/util2d.h"
@@ -62,14 +66,25 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
exactSyncDepth_(0),
exactSyncDisparity_(0)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
int qos = 0;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
@@ -120,33 +135,33 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_);
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
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(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "depth/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "depth/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
disparitySub_.subscribe(this, "disparity/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
disparitySub_.subscribe(this, "disparity/image", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudXYZ::~PointCloudXYZ()
rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
+35 -18
View File
@@ -31,10 +31,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_conversions/MsgConversion.h>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#include <image_geometry/pinhole_camera_model.hpp>
#include <image_geometry/stereo_camera_model.hpp>
#endif
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/util2d.h"
@@ -67,12 +73,23 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
{
bool approxSync = true;
std::string roiStr;
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
int qos = 0;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
maxDepth_ = this->declare_parameter("max_depth", maxDepth_);
@@ -135,49 +152,49 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval > 0.0)
approxSyncStereo_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
exactSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/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());
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageDisparitySub_.subscribe(this, "disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDisparitySub_.subscribe(this, "disparity", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageLeft_.subscribe(this, "left/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRight_.subscribe(this, "right/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudXYZRGB::~PointCloudXYZRGB()
rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
+19 -7
View File
@@ -60,10 +60,21 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
int qos = 0;
bool approx = true;
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
@@ -86,7 +97,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
RCLCPP_INFO(this->get_logger(), "Params:");
RCLCPP_INFO(this->get_logger(), " approx=%s", approx?"true":"false");
RCLCPP_INFO(this->get_logger(), " queue_size=%d", queueSize);
RCLCPP_INFO(this->get_logger(), " topic_queue_size=%d", topicQueueSize);
RCLCPP_INFO(this->get_logger(), " sync_queue_size=%d", syncQueueSize);
RCLCPP_INFO(this->get_logger(), " fixed_frame_id=%s", fixedFrameId_.c_str());
RCLCPP_INFO(this->get_logger(), " wait_for_transform=%fs", waitForTransform_);
RCLCPP_INFO(this->get_logger(), " fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
@@ -105,18 +117,18 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
if(approx)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_);
approxSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
fixedFrameId_.clear();
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_);
exactSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
}
pointCloudSub_.subscribe(this, "cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
pointCloudSub_.subscribe(this, "cloud", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
}
PointCloudToDepthImage::~PointCloudToDepthImage()
rtabmap_util/src/nodelets/rgbd_relay.cpp
+4
View File
@@ -32,7 +32,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/image_encodings.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap_conversions/MsgConversion.h"
rtabmap_util/src/nodelets/rgbd_split.cpp
+4 -3
View File
@@ -27,7 +27,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_util/rgbd_split.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
namespace rtabmap_util
{
@@ -35,12 +39,9 @@ namespace rtabmap_util
RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
Node("rgbd_split", options)
{
int queueSize = 10;
int qos = 0;
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
rtabmap_viz/CMakeLists.txt
+2
View File
@@ -16,6 +16,8 @@ find_package(rtabmap_msgs REQUIRED)
find_package(rtabmap_sync REQUIRED)
find_package(tf2 REQUIRED)
find_package(RTABMap COMPONENTS gui REQUIRED)
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/include
)
rtabmap_viz/src/GuiWrapper.cpp
+18 -18
View File
@@ -142,32 +142,32 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
UEventsManager::addHandler(this);
UEventsManager::addHandler(mainWindow_);
republishNodeDataPub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("republish_node_data", 1);
republishNodeDataPub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>(rtabmapNodeName_+"/republish_node_data", 1);
if(subscribeInfoOnly)
{
RCLCPP_INFO(this->get_logger(), "rtabmap_viz: subscribe_info_only=true");
infoOnlyTopic_ = this->create_subscription<rtabmap_msgs::msg::Info>("info", 1, std::bind(&GuiWrapper::infoCallback, this, std::placeholders::_1));
infoOnlyTopic_ = this->create_subscription<rtabmap_msgs::msg::Info>("info", rclcpp::QoS(this->getTopicQueueSize()), std::bind(&GuiWrapper::infoCallback, this, std::placeholders::_1));
}
else
{
infoTopic_.subscribe(this, "info");
mapDataTopic_.subscribe(this, "mapData");
infoTopic_.subscribe(this, "info", rclcpp::QoS(this->getTopicQueueSize()).get_rmw_qos_profile());
mapDataTopic_.subscribe(this, "mapData", rclcpp::QoS(this->getTopicQueueSize()).get_rmw_qos_profile());
infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
MyInfoMapSyncPolicy(this->getQueueSize()),
MyInfoMapSyncPolicy(this->getSyncQueueSize()),
infoTopic_,
mapDataTopic_);
infoMapSync_->registerCallback(std::bind(&GuiWrapper::infoMapCallback, this, std::placeholders::_1, std::placeholders::_2));
}
goalTopic_.subscribe(this, "goal_node");
pathTopic_.subscribe(this, "global_path");
goalTopic_.subscribe(this, "goal_node", rclcpp::QoS(this->getTopicQueueSize()).get_rmw_qos_profile());
pathTopic_.subscribe(this, "global_path", rclcpp::QoS(this->getTopicQueueSize()).get_rmw_qos_profile());
goalPathSync_ = new message_filters::Synchronizer<MyGoalPathSyncPolicy>(
MyGoalPathSyncPolicy(this->getQueueSize()),
MyGoalPathSyncPolicy(this->getSyncQueueSize()),
goalTopic_,
pathTopic_);
goalPathSync_->registerCallback(std::bind(&GuiWrapper::goalPathCallback, this, std::placeholders::_1, std::placeholders::_2));
goalReachedTopic_ = this->create_subscription<std_msgs::msg::Bool>("goal_reached", 5, std::bind(&GuiWrapper::goalReachedCallback, this, std::placeholders::_1));
goalReachedTopic_ = this->create_subscription<std_msgs::msg::Bool>("goal_reached", rclcpp::QoS(topicQueueSize_), std::bind(&GuiWrapper::goalReachedCallback, this, std::placeholders::_1));
setupCallbacks(*this); // do it at the end
}
@@ -369,7 +369,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
{
if(!callEmptyService("reset"))
if(!callEmptyService(rtabmapNodeName_+"/reset"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"reset\" service");
}
@@ -390,7 +390,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
callEmptyService("pause_odom");
// Pause rtabmap
if(!callEmptyService("pause"))
if(!callEmptyService(rtabmapNodeName_+"/pause"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"pause\" service");
}
@@ -398,7 +398,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
else if(cmd == rtabmap::RtabmapEventCmd::kCmdResume)
{
// Resume rtabmap
if(!callEmptyService("resume"))
if(!callEmptyService(rtabmapNodeName_+"/resume"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"resume\" service");
}
@@ -418,7 +418,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)
{
if(!callEmptyService("trigger_new_map"))
if(!callEmptyService(rtabmapNodeName_+"/trigger_new_map"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"trigger_new_map\" service");
}
@@ -429,7 +429,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
UASSERT(cmdEvent->value2().isBool());
UASSERT(cmdEvent->value3().isBool());
if(!callMapDataService("get_map_data", cmdEvent->value1().toBool(), cmdEvent->value2().toBool(), cmdEvent->value3().toBool()))
if(!callMapDataService(rtabmapNodeName_+"/get_map_data", cmdEvent->value1().toBool(), cmdEvent->value2().toBool(), cmdEvent->value3().toBool()))
{
this->post(new RtabmapEvent3DMap(1)); // service error
}
@@ -438,7 +438,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
{
UASSERT(cmdEvent->value1().isStr() || cmdEvent->value1().isInt() || cmdEvent->value1().isUInt());
auto client = this->create_client<rtabmap_msgs::srv::SetGoal>("set_goal");
auto client = this->create_client<rtabmap_msgs::srv::SetGoal>(rtabmapNodeName_+"/set_goal");
if(client->wait_for_service(std::chrono::seconds(1)))
{
auto request = std::make_shared<rtabmap_msgs::srv::SetGoal::Request>();
@@ -468,7 +468,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
{
if(!callEmptyService("cancel_goal"))
if(!callEmptyService(rtabmapNodeName_+"/cancel_goal"))
{
RCLCPP_ERROR(this->get_logger(), "Can't call \"cancel_goal\" service");
}
@@ -478,7 +478,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
UASSERT(cmdEvent->value1().isStr());
UASSERT(cmdEvent->value2().isUndef() || cmdEvent->value2().isInt() || cmdEvent->value2().isUInt());
auto client = this->create_client<rtabmap_msgs::srv::SetLabel>("set_label");
auto client = this->create_client<rtabmap_msgs::srv::SetLabel>(rtabmapNodeName_+"/set_label");
if(client->wait_for_service(std::chrono::seconds(1)))
{
auto request = std::make_shared<rtabmap_msgs::srv::SetLabel::Request>();
@@ -495,7 +495,7 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
else if(cmd == rtabmap::RtabmapEventCmd::kCmdRemoveLabel)
{
UASSERT(cmdEvent->value1().isStr());
auto client = this->create_client<rtabmap_msgs::srv::RemoveLabel>("remove_label");
auto client = this->create_client<rtabmap_msgs::srv::RemoveLabel>(rtabmapNodeName_+"/remove_label");
if(client->wait_for_service(std::chrono::seconds(1)))
{
auto request = std::make_shared<rtabmap_msgs::srv::RemoveLabel::Request>();