diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 25294bd72..0d820fb4a 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/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; diff --git a/rtabmap_examples/launch/depthai.launch.py b/rtabmap_examples/launch/depthai.launch.py new file mode 100644 index 000000000..8918bba04 --- /dev/null +++ b/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 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) + ]) diff --git a/rtabmap_examples/launch/realsense_d400.launch.py b/rtabmap_examples/launch/realsense_d400.launch.py index 79c1ffac2..d2e47aff2 100644 --- a/rtabmap_examples/launch/realsense_d400.launch.py +++ b/rtabmap_examples/launch/realsense_d400.launch.py @@ -2,16 +2,16 @@ # A realsense D400 series # Install realsense2 ros2 package (make sure you have this patch: https://github.com/IntelRealSense/realsense-ros/issues/2564#issuecomment-1336288238) # Example: -# $ ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true -# # $ ros2 launch rtabmap_examples realsense_d400.launch.py -# OR -# $ ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false + +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -27,7 +27,18 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + # Make sure IR emitter is enabled + SetParameter(name='depth_module.emitter_enabled', value=1), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'align_depth.enable': 'true', + 'rgb_camera.profile': '640x360x30'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, diff --git a/rtabmap_examples/launch/realsense_d435i_color.launch.py b/rtabmap_examples/launch/realsense_d435i_color.launch.py index 4839874b3..298b800f4 100644 --- a/rtabmap_examples/launch/realsense_d435i_color.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_color.launch.py @@ -6,10 +6,14 @@ # # $ ros2 launch rtabmap_examples realsense_d435i_color.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -23,11 +27,26 @@ def generate_launch_description(): ('imu', '/imu/data'), ('rgb/image', '/camera/color/image_raw'), ('rgb/camera_info', '/camera/color/camera_info'), - ('depth/image', '/camera/realigned_depth_to_color/image_raw')] + ('depth/image', '/camera/aligned_depth_to_color/image_raw')] return LaunchDescription([ - # Nodes to launch + # Make sure IR emitter is enabled + SetParameter(name='depth_module.emitter_enabled', value=1), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': '1', + 'align_depth.enable': 'true', + 'enable_sync': 'true', + 'rgb_camera.profile': '640x360x30'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, @@ -43,26 +62,7 @@ def generate_launch_description(): package='rtabmap_viz', executable='rtabmap_viz', output='screen', parameters=parameters, remappings=remappings), - - # Because of this issue: https://github.com/IntelRealSense/realsense-ros/issues/2564 - # Generate point cloud from not aligned depth - Node( - package='rtabmap_util', executable='point_cloud_xyz', output='screen', - parameters=[{'approx_sync':False}], - remappings=[('depth/image', '/camera/depth/image_rect_raw'), - ('depth/camera_info', '/camera/depth/camera_info'), - ('cloud', '/camera/cloud_from_depth')]), - - # Generate aligned depth to color camera from the point cloud above - Node( - package='rtabmap_util', executable='pointcloud_to_depthimage', output='screen', - parameters=[{ 'decimation':2, - 'fixed_frame_id':'camera_link', - 'fill_holes_size':1}], - remappings=[('camera_info', '/camera/color/camera_info'), - ('cloud', '/camera/cloud_from_depth'), - ('image_raw', '/camera/realigned_depth_to_color/image_raw')]), - + # Compute quaternion of the IMU Node( package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen', diff --git a/rtabmap_examples/launch/realsense_d435i_infra.launch.py b/rtabmap_examples/launch/realsense_d435i_infra.launch.py index d4a2116f3..140e995b5 100644 --- a/rtabmap_examples/launch/realsense_d435i_infra.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_infra.launch.py @@ -2,15 +2,16 @@ # A realsense D435i # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: -# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true -# $ ros2 param set /camera/camera depth_module.emitter_enabled 0 -# # $ ros2 launch rtabmap_examples realsense_d435i_infra.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -28,7 +29,22 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + #Hack to disable IR emitter + SetParameter(name='depth_module.emitter_enabled', value=0), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': '1', + 'enable_infra1': 'true', + 'enable_infra2': 'true', + 'enable_sync': 'true'}.items(), + ), + Node( package='rtabmap_odom', executable='rgbd_odometry', output='screen', parameters=parameters, diff --git a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py index 43f93a610..ce8795547 100644 --- a/rtabmap_examples/launch/realsense_d435i_stereo.launch.py +++ b/rtabmap_examples/launch/realsense_d435i_stereo.launch.py @@ -2,15 +2,16 @@ # A realsense D435i # Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera) # Example: -# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true -# $ ros2 param set /camera/camera depth_module.emitter_enabled 0 -# # $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py +import os + +from ament_index_python.packages import get_package_share_directory + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): parameters=[{ @@ -28,7 +29,22 @@ def generate_launch_description(): return LaunchDescription([ - # Nodes to launch + #Hack to disable IR emitter + SetParameter(name='depth_module.emitter_enabled', value=0), + + # Launch camera driver + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('realsense2_camera'), 'launch'), + '/rs_launch.py']), + launch_arguments={'enable_gyro': 'true', + 'enable_accel': 'true', + 'unite_imu_method': '1', + 'enable_infra1': 'true', + 'enable_infra2': 'true', + 'enable_sync': 'true'}.items(), + ), + Node( package='rtabmap_odom', executable='stereo_odometry', output='screen', parameters=parameters, diff --git a/rtabmap_examples/launch/vlp16.launch.py b/rtabmap_examples/launch/vlp16.launch.py index dfc6927d9..84bdf92ec 100644 --- a/rtabmap_examples/launch/vlp16.launch.py +++ b/rtabmap_examples/launch/vlp16.launch.py @@ -1,15 +1,16 @@ # Example: -# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py -# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py -# -# SLAM: # $ ros2 launch rtabmap_examples vlp16.launch.py +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): @@ -28,6 +29,17 @@ def generate_launch_description(): description='Enable lidar deskewing'), # Nodes to launch + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_driver'), 'launch'), + '/velodyne_driver_node-VLP16-launch.py']), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('velodyne_pointcloud'), 'launch'), + '/velodyne_transform_node-VLP16-launch.py']), + ), + Node( package='rtabmap_odom', executable='icp_odometry', output='screen', parameters=[{ @@ -57,18 +69,7 @@ def generate_launch_description(): remappings=[ ('scan_cloud', '/velodyne_points') ]), - - Node( - package='rtabmap_util', executable='point_cloud_assembler', output='screen', - parameters=[{ - 'max_clouds':10, - 'fixed_frame_id':'', - 'use_sim_time':use_sim_time, - }], - remappings=[ - ('cloud', 'odom_filtered_input_scan') - ]), - + Node( package='rtabmap_slam', executable='rtabmap', output='screen', parameters=[{ @@ -102,7 +103,7 @@ def generate_launch_description(): 'Icp/CorrespondenceRatio': '0.2' }], remappings=[ - ('scan_cloud', 'assembled_cloud') + ('scan_cloud', 'odom_filtered_input_scan') ], arguments=[ '-d' # This will delete the previous database (~/.ros/rtabmap.db) diff --git a/rtabmap_examples/launch/zed.launch.py b/rtabmap_examples/launch/zed.launch.py new file mode 100644 index 000000000..0b89cec50 --- /dev/null +++ b/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 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) + ]) diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py index fda87a746..64e70aff5 100644 --- a/rtabmap_launch/launch/rtabmap.launch.py +++ b/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,12 +377,15 @@ 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, "voxel_size": 0.0, "approx_sync": LaunchConfiguration('approx_sync'), - "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval') + "approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'), + "qos": LaunchConfiguration('qos_image'), + "qos_camera_info": LaunchConfiguration('qos_camera_info') }], remappings=[ ('left/image', LaunchConfiguration('left_image_topic_relay')), @@ -418,8 +430,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('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('topic_queue_size', default_value='10', description='Queue size of individual topic subscribers.'), + DeclareLaunchArgument('queue_size', default_value='2', 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/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index ae2f7d5fd..b42ebbe0c 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -59,7 +60,7 @@ class Odometry; namespace rtabmap_odom { -class OdometryROS : public rclcpp::Node +class OdometryROS : public rclcpp::Node, public UThread { public: @@ -98,12 +99,17 @@ class OdometryROS : public rclcpp::Node private: + virtual void mainLoop(); + virtual void mainLoopKill(); virtual void updateParameters(rtabmap::ParametersMap &) {} virtual void onOdomInit() {} void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg); void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity()); +protected: + rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_; + private: rtabmap::Odometry * odometry_; @@ -147,6 +153,14 @@ class OdometryROS : public rclcpp::Node std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; rclcpp::Subscription::SharedPtr imuSub_; + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + + // Safe-threading + UMutex imuMutex_; + UMutex dataMutex_; + USemaphore dataReady_; + rtabmap::SensorData dataToProcess_; + std_msgs::msg::Header dataHeaderToProcess_; bool paused_; int resetCountdown_; @@ -166,7 +180,6 @@ class OdometryROS : public rclcpp::Node bool waitIMUToinit_; bool imuProcessed_; std::map imus_; - std::pair bufferedData_; std::string configPath_; rtabmap::Transform initialPose_; diff --git a/rtabmap_odom/src/ICPOdometryNode.cpp b/rtabmap_odom/src/ICPOdometryNode.cpp index da85a658e..f83264d02 100644 --- a/rtabmap_odom/src/ICPOdometryNode.cpp +++ b/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(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 8605cf384..caa436a42 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -97,6 +97,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o initialPose_(Transform::getIdentity()), ulogToRosout_(this) { + dataCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + int qos = this->declare_parameter("qos", (int)qos_); qos_ = (rmw_qos_reliability_policy_t)qos; @@ -204,6 +206,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o OdometryROS::~OdometryROS() { + this->join(true); delete odometry_; } @@ -365,14 +368,19 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams) Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_); if(waitIMUToinit_) { - int queueSize = 10; - this->get_parameter_or("queue_size", queueSize, queueSize); + imuCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions options; + 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("imu", rclcpp::QoS(queueSize*5).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1)); + imuSub_ = create_subscription("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); } + this->start(); + onOdomInit(); } @@ -408,6 +416,8 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) if(!this->isPaused()) { double stamp = rtabmap_conversions::timestampFromROS(msg->header.stamp); + //RCLCPP_WARN(get_logger(), "Received imu: %f delay=%f", stamp, (now() - msg->header.stamp).seconds()); + rtabmap::Transform localTransform = rtabmap::Transform::getIdentity(); if(this->frameId().compare(msg->header.frame_id) != 0) { @@ -428,18 +438,12 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), localTransform); + UScopeMutex m(imuMutex_); imus_.insert(std::make_pair(stamp, imu)); - //RCLCPP_WARN(get_logger(), "Received imu: %f", stamp); - - if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp()) - { - SensorData data = bufferedData_.first; - bufferedData_.first = SensorData(); - processData(data, bufferedData_.second); - } if(imus_.size() > 1000) { + RCLCPP_WARN(this->get_logger(), "Dropping imu data!"); imus_.erase(imus_.begin()); } } @@ -447,45 +451,78 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg) void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header) { - if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) + //RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds()); + if(dataMutex_.lockTry() == 0) + { + dataToProcess_ = data; + dataHeaderToProcess_ = header; + dataReady_.release(); + dataMutex_.unlock(); + } + else + { + RCLCPP_DEBUG(get_logger(), "Dropping image/scan data"); + } +} + +void OdometryROS::mainLoopKill() +{ + // in case we were waiting, unblock thread + dataReady_.release(); +} + +void OdometryROS::mainLoop() +{ + dataReady_.acquire(); + + if(!this->isRunning()) { - RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name()); + // thread killed return; } - if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) + UScopeMutex lock(dataMutex_); + + // aliases + SensorData & data = dataToProcess_; + std_msgs::msg::Header & header = dataHeaderToProcess_; + + std::vector > imus; { - //RCLCPP_WARN(get_logger(), "No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", timestampFromROS(header.stamp)); + UScopeMutex m(imuMutex_); + + if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) + { + RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name()); + return; + } - // keep in cache to process later when we will receive imu msgs - if(bufferedData_.first.isValid()) + if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp))) { - RCLCPP_ERROR(this->get_logger(), "Overwriting previous data! Make sure IMU is " - "published faster than data rate. (last image stamp " - "buffered=%f and new one is %f, last imu stamp received=%f)", - bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + return; } - bufferedData_.first = data; - bufferedData_.second = header; - return; - } - // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) - std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); - if(iterEnd!= imus_.end()) - { - ++iterEnd; - } - for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) + std::map::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp)); + if(iterEnd!= imus_.end()) + { + ++iterEnd; + } + for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + { + imus.push_back(*iter); + imus_.erase(iter++); + } + } // end imu lock + + for(size_t i=0; ifirst); - SensorData dataIMU(iter->second, 0, iter->first); + SensorData dataIMU(imus[i].second, 0, imus[i].first); odometry_->process(dataIMU); - imus_.erase(iter++); imuProcessed_ = true; } - //RCLCPP_WARN(get_logger(), "img callback: process image %f", timestampFromROS(header.stamp)); - Transform groundTruth; if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) { @@ -1017,20 +1054,21 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h odomSensorDataCompressedPub_->publish(msg); } + double delay = (now()-header.stamp).seconds(); if(visParams_) { if(icpParams_) { - RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); + RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); } else { - RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); + RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); } } else // if(icpParams_) { - RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds()); + RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (now()-timeStart).seconds(), delay); } statusDiagnostic_.setStatus(pose.isNull()); @@ -1067,14 +1105,18 @@ void OdometryROS::resetToPose( void OdometryROS::reset(const Transform & pose) { + UScopeMutex lock(dataMutex_); odometry_->reset(pose); guess_.setNull(); guessPreviousPose_.setNull(); previousStamp_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; - bufferedData_.first= SensorData(); + dataToProcess_ = SensorData(); + dataHeaderToProcess_ = std_msgs::msg::Header(); + imuMutex_.lock(); imus_.clear(); + imuMutex_.unlock(); this->flushCallbacks(); } diff --git a/rtabmap_odom/src/RGBDOdometryNode.cpp b/rtabmap_odom/src/RGBDOdometryNode.cpp index ad203b994..270d9eded 100644 --- a/rtabmap_odom/src/RGBDOdometryNode.cpp +++ b/rtabmap_odom/src/RGBDOdometryNode.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(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/StereoOdometryNode.cpp b/rtabmap_odom/src/StereoOdometryNode.cpp index d8976b7bf..60a729ce5 100644 --- a/rtabmap_odom/src/StereoOdometryNode.cpp +++ b/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(options)); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 4babddd39..4476870ce 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -97,8 +97,11 @@ void ICPOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false"); RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false"); - scan_sub_ = create_subscription("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1)); - cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1)); + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; + + scan_sub_ = create_subscription("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options); + cloud_sub_ = create_subscription("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options); filtered_scan_pub_ = create_publisher("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos())); diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index 4a51d929d..697da9634 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/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); @@ -125,29 +125,32 @@ void RGBDOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false"); + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; + std::string subscribedTopic; std::string subscribedTopicsMsg; if(subscribeRGBD) { if(rgbdCameras >= 2) { - 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()); + rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).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(), options); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).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(), options); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).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(), options); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).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(), options); } if(rgbdCameras == 2) @@ -327,7 +330,7 @@ void RGBDOdometry::onOdomInit() } else if(rgbdCameras == 0) { - rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1)); + rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1), options); subscribedTopic = rgbdxSub_->get_topic_name(); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -336,7 +339,7 @@ void RGBDOdometry::onOdomInit() } else { - rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1)); + rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1), options); subscribedTopic = rgbdSub_->get_topic_name(); subscribedTopicsMsg = @@ -348,9 +351,9 @@ void RGBDOdometry::onOdomInit() else { image_transport::TransportHints hints(this); - 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()); + image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options); if(approxSync) { @@ -366,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()); diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 7a291ec44..c6517eba3 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/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); @@ -120,29 +120,32 @@ void StereoOdometry::onOdomInit() RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false"); + rclcpp::SubscriptionOptions options; + options.callback_group = dataCallbackGroup_; + std::string subscribedTopic; std::string subscribedTopicsMsg; if(subscribeRGBD) { if(rgbdCameras >= 2) { - 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()); + rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).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(), options); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).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(), options); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).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(), options); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).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(), options); } if(rgbdCameras == 2) @@ -323,7 +326,7 @@ void StereoOdometry::onOdomInit() } else if(rgbdCameras == 0) { - rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1)); + rgbdxSub_ = create_subscription("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1), options); subscribedTopic = rgbdxSub_->get_topic_name(); subscribedTopicsMsg = @@ -333,7 +336,7 @@ void StereoOdometry::onOdomInit() } else { - rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1)); + rgbdSub_ = create_subscription("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1), options); subscribedTopic = rgbdSub_->get_topic_name(); subscribedTopicsMsg = @@ -345,10 +348,10 @@ void StereoOdometry::onOdomInit() else { image_transport::TransportHints hints(this); - 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()); + imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options); + cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options); + cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options); if(approxSync) { @@ -364,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(), diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index c036cd685..e3cd35489 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -2316,7 +2316,7 @@ 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, @@ -2324,6 +2324,7 @@ void CoreWrapper::process( 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)); diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index 49fd026dd..746510a5a 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -31,8 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : - topicQueueSize_(1), - syncQueueSize_(10), + topicQueueSize_(10), + syncQueueSize_(2), approxSync_(true), subscribedToDepth_(!gui), subscribedToStereo_(false), @@ -699,8 +699,12 @@ void CommonDataSubscriber::setupCallbacks( uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " "header are set. If topics are coming from different computers, make sure " - "the clocks of the computers are synchronized (\"ntpdate\"). %s%s", + "the clocks of the computers are synchronized (\"ntpdate\"). Ajusting " + "topic_queue_size (%d) and sync_queue_size (%d) can also help for better " + "synchronization if framerates and/or delays are different. %s%s", name_.c_str(), + topicQueueSize_, + syncQueueSize_, approxSync_? 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.", diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 9c0cf0be3..eaafadf30 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -51,8 +51,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : approxSync_(0), exactSync_(0) { - int topicQueueSize = 1; - int syncQueueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 2; bool approxSync = true; int qos = 0; double approxSyncMaxInterval = 0.0; @@ -115,8 +115,11 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ ros2 topic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different. %s%s", this->get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"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())); diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 0104b1691..f8b054955 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -53,8 +53,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : approxSyncDepth_(0), exactSyncDepth_(0) { - int topicQueueSize = 1; - int syncQueueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 2; bool approxSync = true; double approxSyncMaxInterval = 0.0; int qos = 0; @@ -128,8 +128,11 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different. %s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"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())); diff --git a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp index c78d6d581..c5d150e09 100644 --- a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp @@ -42,8 +42,8 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : SYNC_INIT(rgbd7), SYNC_INIT(rgbd8) { - int topicQueueSize = 1; - int syncQueueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 2; bool approxSync = true; int rgbdCameras = 2; double approxSyncMaxInterval = 0.0; @@ -151,8 +151,11 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init("", uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different.%s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"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())); diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index eed21804c..1978731df 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -50,8 +50,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : approxSync_(0), exactSync_(0) { - int topicQueueSize = 1; - int syncQueueSize = 10; + int topicQueueSize = 10; + int syncQueueSize = 2; bool approxSync = false; double approxSyncMaxInterval = 0.0; int qos = 0; @@ -117,8 +117,11 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : syncDiagnostic_->init(imageLeftSub_.getSubscriber().getTopic(), uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are " "published (\"$ rostopic hz my_topic\") and the timestamps in their " - "header are set. %s%s", + "header are set. Ajusting topic_queue_size (%d) and sync_queue_size (%d) " + "can also help for better synchronization if framerates and/or delays are different.%s%s", get_name(), + topicQueueSize, + syncQueueSize, approxSync?"":"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())); diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp index e7a11de5a..be2448af4 100644 --- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp +++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp @@ -45,10 +45,8 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) : int qos = 0; qos = this->declare_parameter("qos", qos); - auto node = std::shared_ptr(this, [](auto *) {}); - image_transport::ImageTransport it(node); - pub32f_ = image_transport::create_publisher(node.get(), "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - pub16u_ = image_transport::create_publisher(node.get(), "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); sub_ = create_subscription("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1)); } diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index 95548bf9b..0443c0b6e 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -107,10 +107,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_); RCLCPP_INFO(this->get_logger(), " upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_); - auto node = std::shared_ptr(this, [](auto *) {}); - image_transport::ImageTransport it(node); - depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm - depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters + depthImage16Pub_ = image_transport::create_camera_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm + depthImage32Pub_ = image_transport::create_camera_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters pointCloudTransformedPub_ = create_publisher("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos)); cameraInfo16Pub_ = create_publisher(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); cameraInfo32Pub_ = create_publisher(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo)); diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index 80af31189..c0a9fc2b0 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -46,10 +46,8 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : rgbdImageSub_ = create_subscription("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1)); - auto node = std::shared_ptr(this, [](auto *) {}); - image_transport::ImageTransport it(node); - rgbPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); - depthPub_ = image_transport::create_camera_publisher(node.get(), std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + rgbPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); + depthPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); } diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 8cb983273..0f8cb4100 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -335,7 +335,6 @@ bool GuiWrapper::handleEvent(UEvent * anEvent) const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters(); rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters(); std::vector rosParameters; - auto node = rclcpp::Node::make_shared("rtabmap_viz"); for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i) { //save only parameters with valid names