merged master->ros2

introlab · Jun 13, 2024 · d3d874c · d3d874c
2 parents 911e4e8 + ae44e1a
commit d3d874c
Show file tree

Hide file tree

Showing 35 changed files with 857 additions and 771 deletions.
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
@@ -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'"
+}
diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py
@@ -45,14 +45,15 @@ 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.'),
         DeclareLaunchArgument('qos_odom',        default_value=LaunchConfiguration('qos'), description='Specific QoS used for odometry input data: 0=system default, 1=Reliable, 2=Best Effort.'),
         DeclareLaunchArgument('qos_user_data',   default_value=LaunchConfiguration('qos'), description='Specific QoS used for user input data: 0=system default, 1=Reliable, 2=Best Effort.'),
         DeclareLaunchArgument('qos_imu',         default_value=LaunchConfiguration('qos'), description='Specific QoS used for imu input data: 0=system default, 1=Reliable, 2=Best Effort.'),
         DeclareLaunchArgument('qos_gps',         default_value=LaunchConfiguration('qos'), description='Specific QoS used for gps input data: 0=system default, 1=Reliable, 2=Best Effort.'),
-
+        
         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
@@ -88,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')}],
@@ -122,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=[
@@ -169,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'),
@@ -203,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'),
@@ -237,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),
@@ -277,7 +283,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'),
@@ -329,7 +336,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'),
@@ -408,7 +416,8 @@ def generate_launch_description():
         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.'),

diff --git a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
@@ -148,7 +148,8 @@ class RGBDOdometry : public rtabmap_odom::OdometryROS
 	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_;
 };
 

diff --git a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
@@ -146,7 +146,8 @@ class StereoOdometry : public rtabmap_odom::OdometryROS
 	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_;
 };