Updated default topic and sync queue_size inside nodes. Exposing topi…

…c and sync queue size params in warning when cannot synchronize. Added zed and depthai examples. Odom: Fixed imu callback group, added multi-threaded executors for all odometry nodes.

rtabmap_conversions/src/MsgConversion.cpp

@@ -1988,12 +1988,6 @@ rtabmap::Transform getTransform(
 {
 	// TF ready?
 	rtabmap::Transform transform;
-	std::string errString;
-	if(!tfBuffer.canTransform(fromFrameId, toFrameId,  tf2_ros::fromMsg(stamp), tf2::durationFromSec(waitForTransform), &errString))
-	{
-		UWARN("(can transform %s -> %s?) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), errString.c_str(), waitForTransform);
-		return rtabmap::Transform();
-	}
 	try
 	{
 		geometry_msgs::msg::TransformStamped tmp;

rtabmap_examples/launch/depthai.launch.py

@@ -0,0 +1,72 @@
+# Requirements:
+#   A OAK-D camera
+#   Install depthai-ros package (https://github.com/luxonis/depthai-ros)
+# Example:
+#   $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    parameters=[{'frame_id':'oak-d-base-frame',
+                 'subscribe_rgbd':True,
+                 'subscribe_odom_info':True,
+                 'approx_sync':False,
+                 'wait_imu_to_init':True}]
+
+    remappings=[('imu', '/imu/data')]
+
+    return LaunchDescription([
+
+        # Launch depthai camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('depthai_examples'), 'launch'),
+                '/stereo_inertial_node.launch.py']),
+                launch_arguments={'depth_aligned': 'false',
+                                  'enableRviz': 'false',
+                                  'monoResolution': '400p'}.items(),
+        ),
+
+        # Sync right/depth/camera_info together
+        Node(   
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=parameters,
+            remappings=[('rgb/image', '/right/image_rect'),
+                        ('rgb/camera_info', '/right/camera_info'),
+                        ('depth/image', '/stereo/depth')]),
+
+        # Compute quaternion of the IMU
+        Node(
+            package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
+            parameters=[{'use_mag': False, 
+                         'world_frame':'enu', 
+                         'publish_tf':False}],
+            remappings=[('imu/data_raw', '/imu')]),
+
+        # Visual odometry
+        Node(
+            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+
+        # VSLAM
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        # Visualization
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=parameters,
+            remappings=remappings)
+    ])

rtabmap_examples/launch/zed.launch.py

@@ -0,0 +1,76 @@
+# Requirements:
+#   A ZED camera
+#   Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper)
+# Example:
+#   $ ros2 launch rtabmap_examples zed.launch.py camera_model:=zed2i
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+import tempfile
+
+def generate_launch_description():
+
+    # Hack to override grab_resolution parameter without changing any files
+    with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
+        zed_override_file.write("---\n"+
+                  "/**:\n"+
+                  "    ros__parameters:\n"+
+                  "        general:\n"+
+                  "            grab_resolution: 'VGA'")
+
+    camera_model = LaunchConfiguration('camera_model'),
+
+    parameters=[{'frame_id':'zed_camera_link',
+                 'subscribe_rgbd':True,
+                 'subscribe_odom_info':True,
+                 'approx_sync':False,
+                 'wait_imu_to_init':True}]
+
+    remappings=[('imu', '/zed/zed_node/imu/data')]
+
+    return LaunchDescription([
+
+        # Launch depthai camera driver
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('zed_wrapper'), 'launch'),
+                '/zed_camera.launch.py']),
+                launch_arguments={'camera_model': camera_model,
+                                  'ros_params_override_path': zed_override_file.name}.items(),
+        ),
+
+        # Sync right/depth/camera_info together
+        Node(   
+            package='rtabmap_sync', executable='rgbd_sync', output='screen',
+            parameters=parameters,
+            remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
+                        ('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
+                        ('depth/image', '/zed/zed_node/depth/depth_registered')]),
+
+        # Visual odometry
+        Node(
+            package='rtabmap_odom', executable='rgbd_odometry', output='screen',
+            parameters=parameters,
+            remappings=remappings),
+
+        # VSLAM
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=parameters,
+            remappings=remappings,
+            arguments=['-d']),
+
+        # Visualization
+        Node(
+            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
+            parameters=parameters,
+            remappings=remappings)
+    ])

rtabmap_launch/launch/rtabmap.launch.py

@@ -85,6 +85,7 @@ def launch_setup(context, *args, **kwargs):
             namespace=LaunchConfiguration('namespace')),
         Node(
             package='rtabmap_sync', executable='rgbd_sync', name="rgbd_sync", output="screen",
+            emulate_tty=True,
             condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
             parameters=[{
                 "approx_sync": LaunchConfiguration('approx_rgbd_sync'),
@@ -120,6 +121,7 @@ def launch_setup(context, *args, **kwargs):
             namespace=LaunchConfiguration('namespace')),
         Node(
             package='rtabmap_sync', executable='stereo_sync', name="stereo_sync", output="screen",
+            emulate_tty=True,
             condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
             parameters=[{
                 "approx_sync": LaunchConfiguration('approx_rgbd_sync'),
@@ -139,6 +141,7 @@ def launch_setup(context, *args, **kwargs):
         # Relay rgbd_image
         Node(
             package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay", output="screen",
+            emulate_tty=True,
             condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' != 'true'"])),
             parameters=[{
                 "qos": LaunchConfiguration('qos_image')}],
@@ -148,6 +151,7 @@ def launch_setup(context, *args, **kwargs):
             namespace=LaunchConfiguration('namespace')),
         Node(
             package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay_uncompress", output="screen",
+            emulate_tty=True,
             condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' == 'true'"])),
             parameters=[{
                 "uncompress": True,
@@ -160,6 +164,7 @@ def launch_setup(context, *args, **kwargs):
         # RGB-D odometry
         Node(
             package='rtabmap_odom', executable='rgbd_odometry', name="rgbd_odometry", output="screen",
+            emulate_tty=True,
             condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' != 'true'"])),
             parameters=[{
                 "frame_id": LaunchConfiguration('frame_id'),
@@ -195,6 +200,7 @@ def launch_setup(context, *args, **kwargs):
         # Stereo odometry
         Node(
             package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen",
+            emulate_tty=True,
             condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])),
             parameters=[{
                 "frame_id": LaunchConfiguration('frame_id'),
@@ -231,6 +237,7 @@ def launch_setup(context, *args, **kwargs):
         # ICP odometry
         Node(
             package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
+            emulate_tty=True,
             condition=IfCondition(LaunchConfiguration('icp_odometry')),
             parameters=[{
                 "frame_id": LaunchConfiguration('frame_id'),
@@ -260,6 +267,7 @@ def launch_setup(context, *args, **kwargs):
 
         Node(
             package='rtabmap_slam', executable='rtabmap', name="rtabmap", output="screen",
+            emulate_tty=True,
             parameters=[{
                 "subscribe_depth": LaunchConfiguration('depth'),
                 "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
@@ -325,6 +333,7 @@ def launch_setup(context, *args, **kwargs):
 
         Node(
             package='rtabmap_viz', executable='rtabmap_viz', name="rtabmap_viz", output='screen',
+            emulate_tty=True,
             parameters=[{
                 "subscribe_depth": LaunchConfiguration('depth'),
                 "subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
@@ -368,6 +377,7 @@ def launch_setup(context, *args, **kwargs):
             arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
         Node(
             package='rtabmap_util', executable='point_cloud_xyzrgb', name="point_cloud_xyzrgb", output='screen',
+            emulate_tty=True,
             condition=IfCondition(LaunchConfiguration("rviz")),
             parameters=[{
                 "decimation": 4,

rtabmap_odom/src/ICPOdometryNode.cpp

@@ -70,7 +70,10 @@ int main(int argc, char **argv)
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;
 	options.arguments(arguments);
-	rclcpp::spin(std::make_shared<rtabmap_odom::ICPOdometry>(options));
+	auto node = std::make_shared<rtabmap_odom::ICPOdometry>(options);
+	rclcpp::executors::MultiThreadedExecutor executor;
+	executor.add_node(node);
+	executor.spin();
 	rclcpp::shutdown();
 	return 0;
 }

rtabmap_odom/src/OdometryROS.cpp

@@ -373,7 +373,7 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
 		options.callback_group = imuCallbackGroup_;
 		int queueSize = this->declare_parameter("imu_queue_size", 200);
 		int qosImu = this->declare_parameter("qos_imu", (int)qos_);
-		imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
+		imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1), options);
 		RCLCPP_INFO(this->get_logger(), "odometry: Subscribing to IMU topic %s", imuSub_->get_topic_name());
 		RCLCPP_INFO(this->get_logger(), "odometry: qos_imu = %d", qosImu);
 		RCLCPP_INFO(this->get_logger(), "odometry: imu_queue_size = %d", queueSize);
@@ -459,6 +459,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 		dataReady_.release();
 		dataMutex_.unlock();
 	}
+	else
+	{
+		RCLCPP_DEBUG(get_logger(), "Dropping image");
+	}
 }
 
 void OdometryROS::mainLoopKill()
@@ -484,6 +488,7 @@ void OdometryROS::mainLoop()
 	SensorData & data = dataToProcess_;
 	std_msgs::msg::Header & header = dataHeaderToProcess_;
 
+	std::vector<std::pair<double, rtabmap::IMU> > imus;
 	{
 		UScopeMutex m(imuMutex_);
 
@@ -507,13 +512,18 @@ void OdometryROS::mainLoop()
 		}
 		for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
 		{
-			SensorData dataIMU(iter->second, 0, iter->first);
-			odometry_->process(dataIMU);
+			imus.push_back(*iter);
 			imus_.erase(iter++);
-			imuProcessed_ = true;
 		}
 	} // end imu lock
 
+	for(size_t i=0; i<imus.size(); ++i)
+	{
+		SensorData dataIMU(imus[i].second, 0, imus[i].first);
+		odometry_->process(dataIMU);
+		imuProcessed_ = true;
+	}
+
 	Transform groundTruth;
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{

rtabmap_odom/src/StereoOdometryNode.cpp

@@ -76,7 +76,10 @@ int main(int argc, char **argv)
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;
 	options.arguments(arguments);
-	rclcpp::spin(std::make_shared<rtabmap_odom::StereoOdometry>(options));
+	auto node = std::make_shared<rtabmap_odom::StereoOdometry>(options);
+	rclcpp::executors::MultiThreadedExecutor executor;
+	executor.add_node(node);
+	executor.spin();
 	rclcpp::shutdown();
 	return 0;
 }

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -63,8 +63,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
 		exactSync5_(0),
 		approxSync6_(0),
 		exactSync6_(0),
-		topicQueueSize_(1),
-		syncQueueSize_(5),
+		topicQueueSize_(10),
+		syncQueueSize_(2),
 		keepColor_(false)
 {
 	OdometryROS::init(false, true, false);
@@ -369,10 +369,12 @@ void RGBDOdometry::onOdomInit()
 		}
 
 		subscribedTopic = image_mono_sub_.getSubscriber().getTopic();
-		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n   %s,\n   %s,\n   %s",
 				get_name(),
 				approxSync?"approx":"exact",
 				approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+				topicQueueSize_,
+				syncQueueSize_,
 				image_mono_sub_.getSubscriber().getTopic().c_str(),
 				image_depth_sub_.getSubscriber().getTopic().c_str(),
 				info_sub_.getSubscriber()->get_topic_name());
@@ -567,6 +569,7 @@ void RGBDOdometry::callback(
 		const sensor_msgs::msg::Image::ConstSharedPtr depth,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
 {
+	RCLCPP_WARN(get_logger(), "Received image callback: %f delay=%f", rtabmap_conversions::timestampFromROS(image->header.stamp), (now() - image->header.stamp).seconds());
 	if(!this->isPaused())
 	{
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -63,8 +63,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
 		exactSync5_(0),
 		approxSync6_(0),
 		exactSync6_(0),
-		topicQueueSize_(1),
-		syncQueueSize_(5),
+		topicQueueSize_(10),
+		syncQueueSize_(2),
 		keepColor_(false)
 {
 	OdometryROS::init(true, true, false);
@@ -367,10 +367,12 @@ void StereoOdometry::onOdomInit()
 		}
 
 		subscribedTopic = imageRectLeft_.getTopic();
-		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n   %s \\\n   %s \\\n   %s \\\n   %s",
 				get_name(),
 				approxSync?"approx":"exact",
 				approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+				topicQueueSize_,
+				syncQueueSize_,
 				imageRectLeft_.getTopic().c_str(),
 				imageRectRight_.getTopic().c_str(),
 				cameraInfoLeft_.getSubscriber()->get_topic_name(),

rtabmap_slam/src/CoreWrapper.cpp

@@ -2316,14 +2316,15 @@ void CoreWrapper::process(
 		{
 			timeRtabmap = timer.ticks();
 		}
-		RCLCPP_INFO(this->get_logger(), "rtabmap (%d): Rate=%.2fs, Limit=%.3fs, Conversion=%.4fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
+		RCLCPP_INFO(this->get_logger(), "rtabmap (%d): Rate=%.2fs, Limit=%.3fs, Conversion=%.4fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs delay=%.4fs (local map=%d, WM=%d)",
 				rtabmap_.getLastLocationId(),
 				rate_>0?1.0f/rate_:0,
 				rtabmap_.getTimeThreshold()/1000.0f,
 				timeMsgConversion,
 				timeRtabmap,
 				timeUpdateMaps,
 				timePublishMaps,
+				(now() - lastPoseStamp_).seconds(),
 				(int)rtabmap_.getLocalOptimizedPoses().size(),
 				rtabmap_.getWMSize()+rtabmap_.getSTMSize());
 		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0));