i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 06:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing topic sync lagging and delay issues, more examples (depthai, zed) (#1206)

Browse Source 
* Fixed odometry latency

* Fixed node ID=0 issues as msgs may not have seq. (#1202)

(cherry picked from commit 097cab0667)

* Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters)

* Changed a log from info->debug

* Changed a log from info->debug

* Updated default topic and sync queue_size inside nodes. Exposing topic 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.

* fixed merge

* Include everything needed in example launch files for simple launch. Small fixes.

---------

Co-authored-by: Borong Yuan <yuanborong@hotmail.com>
This commit is contained in:
matlabbe
2024-09-04 00:46:44 -07:00
committed by GitHub
co-authored by Borong Yuan
parent 268609b0b7
commit c956e3780f
27 changed files with 465 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/src/MsgConversion.cpp
-6
View File
@@ -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
+72
View File
@@ -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)
])
rtabmap_examples/launch/realsense_d400.launch.py
+19 -8
View File
@@ -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,
rtabmap_examples/launch/realsense_d435i_color.launch.py
+25 -25
View File
@@ -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',
rtabmap_examples/launch/realsense_d435i_infra.launch.py
+23 -7
View File
@@ -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,
rtabmap_examples/launch/realsense_d435i_stereo.launch.py
+23 -7
View File
@@ -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,
rtabmap_examples/launch/vlp16.launch.py
+18 -17
View File
@@ -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)
rtabmap_examples/launch/zed.launch.py
+76
View File
@@ -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)
])
rtabmap_launch/launch/rtabmap.launch.py
+15 -3
View File
@@ -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.'),
rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+15 -2
View File
@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/srv/reset_pose.hpp>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/utilite/UThread.h>
#include <boost/thread.hpp>
@@ -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 @@ protected:
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 @@ private:
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::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 @@ private:
bool waitIMUToinit_;
bool imuProcessed_;
std::map<double, rtabmap::IMU> imus_;
std::pair<rtabmap::SensorData, std_msgs::msg::Header > bufferedData_;
std::string configPath_;
rtabmap::Transform initialPose_;
rtabmap_odom/src/ICPOdometryNode.cpp
+4 -1
View File
@@ -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
+84 -42
View File
@@ -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<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(queueSize*5).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);
}
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)
{
RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name());
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())
{
// thread killed
return;
}
if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
{
//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 lock(dataMutex_);
// keep in cache to process later when we will receive imu msgs
if(bufferedData_.first.isValid())
// aliases
SensorData & data = dataToProcess_;
std_msgs::msg::Header & header = dataHeaderToProcess_;
std::vector<std::pair<double, rtabmap::IMU> > imus;
{
UScopeMutex m(imuMutex_);
if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty())
{
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_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name());
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<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
if(iterEnd!= imus_.end())
if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
{
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;
}
// 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<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
if(iterEnd!= imus_.end())
{
++iterEnd;
}
for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
{
imus.push_back(*iter);
imus_.erase(iter++);
}
} // end imu lock
for(size_t i=0; i<imus.size(); ++i)
{
++iterEnd;
}
for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
{
//NODELET_WARN("img callback: process imu %f", iter->first);
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<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(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<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(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<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(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<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(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<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(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<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(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();
}
rtabmap_odom/src/RGBDOdometryNode.cpp
+4 -1
View File
@@ -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::RGBDOdometry>(options));
auto node = std::make_shared<rtabmap_odom::RGBDOdometry>(options);
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}
rtabmap_odom/src/StereoOdometryNode.cpp
+4 -1
View File
@@ -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/icp_odometry.cpp
+5 -2
View File
@@ -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<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(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<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("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<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+19 -14
View File
@@ -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<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).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), 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<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).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), 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());
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+20 -15
View File
@@ -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<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).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), options);
subscribedTopic = rgbdxSub_->get_topic_name();
subscribedTopicsMsg =
@@ -333,7 +336,7 @@ void StereoOdometry::onOdomInit()
}
else
{
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));
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), 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(),
rtabmap_slam/src/CoreWrapper.cpp
+2 -1
View File
@@ -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));
rtabmap_sync/src/CommonDataSubscriber.cpp
+7 -3
View File
@@ -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.",
rtabmap_sync/src/nodelets/rgb_sync.cpp
+6 -3
View File
@@ -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()));
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+6 -3
View File
@@ -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()));
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+6 -3
View File
@@ -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()));
rtabmap_sync/src/nodelets/stereo_sync.cpp
+6 -3
View File
@@ -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()));
rtabmap_util/src/nodelets/disparity_to_depth.cpp
+2 -4
View File
@@ -45,10 +45,8 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) :
int qos = 0;
qos = this->declare_parameter("qos", qos);
auto node = std::shared_ptr<DisparityToDepth>(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<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
}
rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
+2 -4
View File
@@ -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<PointCloudToDepthImage>(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<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cameraInfo16Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
cameraInfo32Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
rtabmap_util/src/nodelets/rgbd_split.cpp
+2 -4
View File
@@ -46,10 +46,8 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
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));
auto node = std::shared_ptr<RGBDSplit>(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());
}
rtabmap_viz/src/GuiWrapper.cpp
-1
View File
@@ -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<rclcpp::Parameter> 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