mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 16:57:46 +08:00
merged master->ros2 (new multi_rgbd_inertial_dataset.launch.py example), fixed rtabmap_viz error on exit, fixed default name of rgbdx_sync node, fixed yaml_to_camera_info.py exit exception
This commit is contained in:
matlabbe
@@ -0,0 +1,162 @@
|
||||
#
|
||||
# Example launch file to run VSLAM on this dataset: https://github.com/seungsang07/multi-rgbd-inertial-dataset
|
||||
#
|
||||
# Requirement(s):
|
||||
# * RTAB-Map should be built with OpenGV support.
|
||||
#
|
||||
# To convert ROS1 bags to ROS2 (https://docs.openvins.com/dev-ros1-to-ros2.html):
|
||||
# sudo pip install rosbags
|
||||
# rosbags-convert --src Indoor.bag --dst Indoor
|
||||
#
|
||||
# Usage:
|
||||
# ros2 launch rtabmap_examples multi_rgbd_inertial_dataset.launch.py
|
||||
# ros2 bag play Indoor/Indoor.db3 --clock
|
||||
#
|
||||
# Note(s):
|
||||
# * Communication performance could be improved using the composable nodes of these nodes instead,
|
||||
# but we are using nodes here to better understand what is going on with rqt_graph.
|
||||
#
|
||||
# To get RMSE after the run (using https://github.com/MichaelGrupp/evo):
|
||||
# rtabmap-export --poses --pose_format 10 ~/.ros/rtabmap.db
|
||||
# evo_ape tum -a -p --plot_mode xy ~/Downloads/GroundTruth/gt_indoor.txt ~/.ros/rtabmap_poses.txt
|
||||
#
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch_ros.actions import SetParameter
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
|
||||
def make_yaml_to_camera_info_node(camera):
|
||||
# the dataset doesn't provide camera_info topics of the camera in the rosbags, so we need to generate them
|
||||
return Node(
|
||||
package='rtabmap_util', executable='yaml_to_camera_info.py', name=f'yaml_to_camera_info_{camera}', output='screen',
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), f'/config/multi_rgbd_inertial_dataset_{camera}.yaml']}],
|
||||
remappings=[
|
||||
('image', 'color/image_raw'),
|
||||
('camera_info', 'color/camera_info')],
|
||||
namespace=f'camera_{camera}')
|
||||
|
||||
def make_rgbd_sync_node(camera):
|
||||
# synchronize topics of each camera together
|
||||
return Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', name=f'rgbd_sync_{camera}', output="screen",
|
||||
parameters=[{"approx_sync": False}],
|
||||
remappings=[
|
||||
("rgb/image", 'color/image_raw'),
|
||||
("depth/image", 'aligned_depth_to_color/image_raw'),
|
||||
("rgb/camera_info", 'color/camera_info'),
|
||||
("rgbd_image", 'rgbd_image')],
|
||||
namespace=f'camera_{camera}')
|
||||
|
||||
def make_static_transform_publisher_node(x, y, z, yaw, pitch, roll, parent_frame, child_frame):
|
||||
return Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', name=f"static_transform_publisher_{parent_frame}_{child_frame}", output='screen',
|
||||
arguments=['--frame-id', parent_frame,
|
||||
'--child-frame-id', child_frame,
|
||||
'--x', str(x),
|
||||
'--y', str(y),
|
||||
'--z', str(z),
|
||||
'--yaw', str(yaw),
|
||||
'--pitch', str(pitch),
|
||||
'--roll', str(roll)])
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
use_imu = LaunchConfiguration('use_imu').perform(context)
|
||||
use_imu = use_imu == 'true' or use_imu == 'True'
|
||||
|
||||
# Synchronize all cameras together
|
||||
rgbdx_sync_node = Node(
|
||||
package='rtabmap_sync', executable='rgbdx_sync', output="screen",
|
||||
parameters=[{
|
||||
"rgbd_cameras": 4,
|
||||
"approx_sync": True,
|
||||
"approx_sync_max_interval": 0.015}],
|
||||
remappings=[
|
||||
("rgbd_image0", '/camera_left/rgbd_image'),
|
||||
("rgbd_image1", '/camera_front/rgbd_image'),
|
||||
("rgbd_image2", '/camera_right/rgbd_image'),
|
||||
("rgbd_image3", '/camera_rear/rgbd_image')],
|
||||
namespace='rtabmap'
|
||||
)
|
||||
|
||||
# RGB-D odometry
|
||||
remappings = []
|
||||
if use_imu:
|
||||
remappings = [("imu", '/imu')]
|
||||
rgbd_odometry_node = Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output="screen",
|
||||
parameters=[{
|
||||
"frame_id": 'base_link',
|
||||
"rgbd_cameras": 0, # make it subscribe to rgbd_images topic from rgbdx_sync
|
||||
"subscribe_rgbd": True,
|
||||
"wait_imu_to_init": use_imu}],
|
||||
remappings=remappings,
|
||||
namespace='rtabmap'
|
||||
)
|
||||
|
||||
# SLAM
|
||||
remappings=[("sensor_data", 'odom_sensor_data/raw')]
|
||||
if(use_imu):
|
||||
remappings.append(('imu', '/imu'))
|
||||
slam_node = Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output="screen",
|
||||
parameters=[{
|
||||
"subscribe_sensor_data": True,
|
||||
"frame_id": 'base_link',
|
||||
"approx_sync": False,
|
||||
"Grid/3D": 'false',
|
||||
"Grid/RayTracing": 'true',
|
||||
"Grid/NormalsSegmentation": 'false',
|
||||
"Grid/MaxGroundHeight": '0.05',
|
||||
"Rtabmap/CreateIntermediateNodes": 'true' # Only to record all odometry poses for trajectory evaluation purpose
|
||||
}],
|
||||
remappings=remappings,
|
||||
arguments=["--delete_db_on_start"],
|
||||
namespace='rtabmap'
|
||||
)
|
||||
|
||||
# Visualization
|
||||
viz_node = Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output="screen",
|
||||
parameters=[{
|
||||
"subscribe_sensor_data": True,
|
||||
"frame_id": 'base_link',
|
||||
"approx_sync": False,
|
||||
"subscribe_odom_info": True
|
||||
}],
|
||||
remappings=[("sensor_data", 'odom_sensor_data/raw')],
|
||||
arguments=["-d", [FindPackageShare('rtabmap_examples'), '/config/multi_rgbd_inertial_dataset.ini']],
|
||||
namespace='rtabmap'
|
||||
)
|
||||
|
||||
return [
|
||||
make_yaml_to_camera_info_node('left'),
|
||||
make_rgbd_sync_node('left'),
|
||||
make_yaml_to_camera_info_node('front'),
|
||||
make_rgbd_sync_node('front'),
|
||||
make_yaml_to_camera_info_node('right'),
|
||||
make_rgbd_sync_node('right'),
|
||||
make_yaml_to_camera_info_node('rear'),
|
||||
make_rgbd_sync_node('rear'),
|
||||
# The dataset doesn't provide /tf or /tf_static for the extrinsics between imu and the cameras, so we add them here
|
||||
make_static_transform_publisher_node(0., 0., 0.22, 3.1415926, 0., 0., 'base_link', 'imu_link'),
|
||||
make_static_transform_publisher_node(-0.099307, -0.208806, 0.024309, 3.108592, -0.051480, -1.592415, 'imu_link', 'camera_left_color_optical_frame'),
|
||||
make_static_transform_publisher_node(-0.435392, 0.022256, 0.053441, 1.532258, -0.007768, -1.580204, 'imu_link', 'camera_front_color_optical_frame'),
|
||||
make_static_transform_publisher_node(-0.063987, 0.212966, 0.032071, -0.010030, 0.021977, -1.553814, 'imu_link', 'camera_right_color_optical_frame'),
|
||||
make_static_transform_publisher_node(0.178893, -0.006307, 0.017677, -1.606156, 0.027545, -1.587179, 'imu_link', 'camera_rear_color_optical_frame'),
|
||||
make_static_transform_publisher_node(0.045872, -0.026775, 0.284806, 3.141063, -0.014869, -0.014969, 'imu_link', 'os_sensor'),
|
||||
rgbdx_sync_node,
|
||||
rgbd_odometry_node,
|
||||
slam_node,
|
||||
viz_node
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('use_imu', default_value='false', description='Use IMU'),
|
||||
SetParameter(name='use_sim_time', value=True),
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
Reference in New Issue
Block a user