MISTLab
Running on a custom RGBD dataset
Hey there, thanks for your great work!
I want to run your method on my RGBD dataset X for evaluation purposes. However, I'm not versed in robotics and ROS in general.
Here is what I have done so far:
- Built and ran the docker container that you provided
- Converted by ROS1 rosbags to ROS2 format with the rosbridge
- I manually downloaded
resnet18_64.pthto/Swarm-SLAM/install/cslam/share/cslam/models/ - Went over related issues in this repo.
Here's my rosbag info:
Bag size: 854.1 MiB
Storage id: sqlite3
Duration: 21.510s
Start: Jul 29 2024 11:46:48.700 (1722253608.700)
End: Jul 29 2024 11:47:10.211 (1722253630.211)
Messages: 1465
Topic information: Topic: /camera/camera_info | Type: sensor_msgs/msg/CameraInfo | Count: 489 | Serialization Format: cdr
Topic: /camera/depth/image_raw | Type: sensor_msgs/msg/Image | Count: 489 | Serialization Format: cdr
Topic: /camera/rgb/image_raw | Type: sensor_msgs/msg/Image | Count: 485 | Serialization Format: cdr
Topic: /rosout | Type: rcl_interfaces/msg/Log | Count: 1 | Serialization Format: cdr
Topic: /rosout_agg | Type: rcl_interfaces/msg/Log | Count: 1 | Serialization Format: cdr
I'm getting confused with the configs. I'll write down what I did and how I understand it (please correct me if I'm wrong).
- I created a config yaml file
cslam_experiments/config/X_rgbd.yamland change only some of the settings:
color_image_topic: "/camera/rgb/image_raw"
depth_image_topic: "/camera/depth/image_raw"
color_camera_info: "/camera/camera_info"
sensor_type: "rgbd"
- I created
cslam_experiments/launch/dataset_experiments/X.launch.py. This launch configuration should start the back-end and front-end (rtabmap odometry) as well as start playing the bag:
def launch_setup(context, *args, **kwargs):
config_path = os.path.join(get_package_share_directory("cslam_experiments"), "config/")
config_file = LaunchConfiguration('config_file').perform(context)
# Params
max_nb_robots = int(LaunchConfiguration('max_nb_robots').perform(context))
dataset = "X_" + LaunchConfiguration('sequence').perform(context)
robot_delay_s = LaunchConfiguration('robot_delay_s').perform(context)
launch_delay_s = LaunchConfiguration('launch_delay_s').perform(context)
rate = float(LaunchConfiguration('rate').perform(context))
# Ajust value according to rate
launch_delay_s = float(launch_delay_s) / rate
cslam_processes = []
odom_processes = []
for i in range(max_nb_robots):
proc = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(get_package_share_directory("cslam_experiments"), "launch", "cslam", "cslam_rgbd.launch.py")),
launch_arguments={
"config_path": config_path,
"config_file": config_file,
"robot_id": str(i),
"namespace": "/r" + str(i),
"max_nb_robots": str(max_nb_robots),
"enable_simulated_rendezvous": LaunchConfiguration('enable_simulated_rendezvous'),
"rendezvous_schedule_file": os.path.join(get_package_share_directory("cslam_experiments"),
"config", "rendezvous", LaunchConfiguration('rendezvous_config').perform(context)),
}.items(),
)
cslam_processes.append(proc)
odom_proc = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('cslam_experiments'), 'launch', 'odometry', 'rtabmap_X_rgbd_odometry.launch.py')),
launch_arguments={
'log_level': "info",
"namespace": "/r" + str(i),
"robot_id": str(i),
}.items(),
)
odom_processes.append(odom_proc)
# Launch schedule
schedule = []
for i in range(max_nb_robots):
schedule.append(PushLaunchConfigurations())
schedule.append(TimerAction(period=float(robot_delay_s) * i, actions=[cslam_processes[i]]))
schedule.append(PopLaunchConfigurations())
schedule.append(PushLaunchConfigurations())
schedule.append(TimerAction(period=float(robot_delay_s) * i, actions=[odom_processes[i]]))
schedule.append(PopLaunchConfigurations())
bag_processes = []
for i in range(max_nb_robots):
# bag_file = os.path.join(
# get_package_share_directory("cslam_experiments"), "data",
# dataset, "X-" + str(i))
bag_file = f"/datasets/X/agent{i}.db3"
bag_proc = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory("cslam_experiments"), "launch", "sensors", "bag_X.launch.py")),
launch_arguments={
"namespace": "/r" + str(i),
"bag_file": bag_file,
"rate": str(rate)
}.items(),
)
bag_processes.append(bag_proc)
for i in range(max_nb_robots):
schedule.append(PushLaunchConfigurations())
schedule.append(
TimerAction(period=float(robot_delay_s) * i + float(launch_delay_s),
actions=[bag_processes[i]]))
schedule.append(PopLaunchConfigurations())
return schedule
- Also, I created a
rtabmap_X_rgbd_odometry.launch.pyto provide odometry to the method:
def launch_setup(context, *args, **kwargs):
parameters=[{
'frame_id': LaunchConfiguration('namespace').perform(context)[1:] + '_link',
'subscribe_depth':True,
'approx_sync':False, # Set to True for OAK-D
}]
remappings=[
('/rgb/image', LaunchConfiguration('namespace').perform(context) + '/camera/rgb/image_raw'),
('/depth/image', LaunchConfiguration('namespace').perform(context) + '/camera/depth/image_raw'),
('/rgb/camera_info', LaunchConfiguration('namespace').perform(context) + '/camera/camera_info'),
]
return [
Node(package='rtabmap_odom', executable='rgbd_odometry', output='screen', name='rgbd_odometry',
parameters=parameters, remappings=remappings)
]
- Finally, I've added a
bag_X.launch.py:
def launch_setup(context, *args, **kwargs):
return [
DeclareLaunchArgument('bag_file', default_value='', description=''),
DeclareLaunchArgument('namespace', default_value='/r0', description=''),
DeclareLaunchArgument('rate', default_value='1.0', description=''),
DeclareLaunchArgument('bag_start_delay', default_value='5.0', description=''),
TimerAction(
period=LaunchConfiguration('bag_start_delay'),
actions=[
ExecuteProcess(
cmd=[
'ros2', 'bag', 'play',
LaunchConfiguration('bag_file').perform(context), '-r', LaunchConfiguration('rate'),
'--remap',
'/camera/rgb/image_raw:=' + LaunchConfiguration('namespace').perform(context) + '/camera/rgb/image_raw',
'/camera/depth/image_raw:=' + LaunchConfiguration('namespace').perform(context) + '/camera/depth/image_raw',
'/camera/camera_info:=' + LaunchConfiguration('namespace').perform(context) + '/camera/camera_info'
],
name='bag',
output='screen',
)
]),
]
I'm getting a warning:
[rgbd_odometry-4] [WARN] [1722260284.003131085] [rgbd_odometry]: rgbd_odometry: 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. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
[rgbd_odometry-4] rgbd_odometry subscribed to (exact sync):
[rgbd_odometry-4] /rgb/image,
[rgbd_odometry-4] /depth/image,
[rgbd_odometry-4] /rgb/camera_info
My rostopic list looks as:
/camera/depth/image_raw
/camera/rgb/image_raw
/cslam/global_descriptors
/cslam/inter_robot_loop_closure
/cslam/inter_robot_matches
/cslam/local_descriptors
/cslam/pose_graph
/cslam/viz/keyframe_pointcloud
/cslam/viz/local_descriptors
/cslam/viz/pose_graph
/depth/image
/diagnostics
/odom
/odom_info
/odom_info_lite
/odom_last_frame
/odom_local_map
/odom_local_scan_map
/odom_rgbd_image
/odom_sensor_data/compressed
/odom_sensor_data/features
/odom_sensor_data/raw
/parameter_events
/r0/color/camera_info
/r0/cslam/current_neighbors
/r0/cslam/current_pose_estimate
/r0/cslam/debug_optimization_result
/r0/cslam/get_current_neighbors
/r0/cslam/get_pose_graph
/r0/cslam/heartbeat
/r0/cslam/intra_robot_loop_closure
/r0/cslam/keyframe_data
/r0/cslam/keyframe_odom
/r0/cslam/local_descriptors_request
/r0/cslam/local_keyframe_match
/r0/cslam/optimized_estimates
/r0/cslam/optimizer_state
/r0/cslam/print_current_estimates
/r0/cslam/reference_frames
/r0/odom
/rgb/camera_info
/rgb/image
/rosout
/tf
/tf_static
However, when I start it, I get a warning:
[rgbd_odometry-4] [ WARN] (2024-07-30 06:51:27.108) MsgConversion.cpp:1994::getTransform() (can transform r0_link -> ?) Invalid frame ID "r0_link" passed to canTransform argument target_frame - frame does not exist. canTransform returned after 0.100673 timeout was 0.1. (wait_for_transform=0.100000)
Also, I get:
[rgbd_odometry-4] [WARN] [1722323112.918425962] [rgbd_odometry]: rgbd_odometry: 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. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
am I missing something?
I've simplified the launch files even more to focus purely on making the odometer work:
Beginning of the X_rgbd.yaml:
/**:
ros__parameters:
frontend:
color_image_topic: "color/image_raw"
color_camera_info: "color/camera_info"
depth_image_topic: "aligned_depth_to_color/image_raw"
sensor_type: "rgbd"
X_rgbd.launch.py launch setup:
def launch_setup(context, *args, **kwargs):
config_path = os.path.join(get_package_share_directory("cslam_experiments"), "config/")
config_file = LaunchConfiguration('config_file').perform(context)
# Params
max_nb_robots = int(LaunchConfiguration('max_nb_robots').perform(context))
dataset = "X_" + LaunchConfiguration('sequence').perform(context)
robot_delay_s = LaunchConfiguration('robot_delay_s').perform(context)
launch_delay_s = LaunchConfiguration('launch_delay_s').perform(context)
rate = float(LaunchConfiguration('rate').perform(context))
# Ajust value according to rate
launch_delay_s = float(launch_delay_s) / rate
cslam_processes = []
odom_processes = []
for i in range(max_nb_robots):
# proc = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(os.path.join(get_package_share_directory("cslam_experiments"), "launch", "cslam", "cslam_rgbd.launch.py")),
# launch_arguments={
# "config_path": config_path,
# "config_file": config_file,
# "robot_id": str(i),
# "namespace": "/r" + str(i),
# "max_nb_robots": str(max_nb_robots),
# "enable_simulated_rendezvous": LaunchConfiguration('enable_simulated_rendezvous'),
# "rendezvous_schedule_file": os.path.join(get_package_share_directory("cslam_experiments"),
# "config", "rendezvous", LaunchConfiguration('rendezvous_config').perform(context)),
# }.items(),
# )
# cslam_processes.append(proc)
odom_proc = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('cslam_experiments'), 'launch', 'odometry', 'rtabmap_X_rgbd_odometry.launch.py')),
launch_arguments={
'log_level': "info",
"namespace": "/r" + str(i),
"robot_id": str(i),
}.items(),
)
odom_processes.append(odom_proc)
# Launch schedule
schedule = []
for i in range(max_nb_robots):
# schedule.append(PushLaunchConfigurations())
# schedule.append(TimerAction(period=float(robot_delay_s) * i, actions=[cslam_processes[i]]))
# schedule.append(PopLaunchConfigurations())
schedule.append(PushLaunchConfigurations())
schedule.append(TimerAction(period=float(robot_delay_s) * i, actions=[odom_processes[i]]))
schedule.append(PopLaunchConfigurations())
bag_processes = []
for i in range(max_nb_robots):
bag_file = f"path_to_bag_file"
bag_proc = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory("cslam_experiments"), "launch", "sensors", "bag_X.launch.py")),
launch_arguments={
"namespace": "/r" + str(i),
"bag_file": bag_file,
"rate": str(rate)
}.items(),
)
bag_processes.append(bag_proc)
for i in range(max_nb_robots):
schedule.append(PushLaunchConfigurations())
schedule.append(TimerAction(period=float(robot_delay_s) * i + float(launch_delay_s), actions=[bag_processes[i]]))
schedule.append(PopLaunchConfigurations())
return schedule
Bag launch file bag_X.launch.py:
def launch_setup(context, *args, **kwargs):
print("Launching bag file")
print(LaunchConfiguration('namespace').perform(context) + '/camera/rgb/image_raw')
return [
DeclareLaunchArgument('bag_file', default_value='', description=''),
DeclareLaunchArgument('namespace', default_value='/r0', description=''),
DeclareLaunchArgument('rate', default_value='1.0', description=''),
DeclareLaunchArgument('bag_start_delay', default_value='5.0', description=''),
TimerAction(
period=LaunchConfiguration('bag_start_delay'),
actions=[
ExecuteProcess(
cmd=[
'ros2', 'bag', 'play',
LaunchConfiguration('bag_file').perform(context), '-r', LaunchConfiguration('rate'),
'--remap',
'/camera/rgb/image_raw:=' + LaunchConfiguration('namespace').perform(context) + '/camera/rgb/image_raw',
'/camera/depth/image_raw:=' + LaunchConfiguration('namespace').perform(context) + '/camera/depth/image_raw',
'/camera/camera_info:=' + LaunchConfiguration('namespace').perform(context) + '/camera/camera_info'
],
name='bag',
output='screen',
)
]),
]
Odometry file:
def launch_setup(context, *args, **kwargs):
parameters=[{
# 'frame_id': LaunchConfiguration('namespace').perform(context)[1:] + '_link',
'frame_id': 'base_link', # for debugging
'subscribe_depth':True,
'approx_sync':False, # Set to True for OAK-D
}]
remappings=[
('/rgb/image', LaunchConfiguration('namespace').perform(context) + '/camera/rgb/image_raw'),
('/depth/image', LaunchConfiguration('namespace').perform(context) + '/camera/depth/image_raw'),
('/rgb/camera_info', LaunchConfiguration('namespace').perform(context) + '/camera/camera_info'),
]
return [
Node(package='rtabmap_odom', executable='rgbd_odometry', output='screen', name='rgbd_odometry',
parameters=parameters, remappings=remappings)
]
When I check the topics they seem to be matching, for example:
root:/Swarm-SLAM# ros2 topic info /r0/camera/depth/image_raw
Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 1
also, echo on a publishing topic gives the timestamps as expected.
However, there are still two issues:
First.
[rgbd_odometry-1] [WARN] [1722341064.212679308] [rgbd_odometry]: rgbd_odometry: 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.
[rgbd_odometry-1] rgbd_odometry subscribed to (approx sync):
Second
[rgbd_odometry-1] [ WARN] (2024-07-30 12:04:23.475) MsgConversion.cpp:1994::getTransform() (can transform base_link -> ?) Invalid frame ID "base_link" passed to canTransform argument target_frame - frame does not exist. canTransform returned after 0.100935 timeout was 0.1. (wait_for_transform=0.100000)
Hi! Thanks for your interest in our work! As correctly figured out the problem is the odometry source. Swarm-SLAM expects odometry as an external input. As an example for users, I suggested to use RTAB-Map because of its versatility and its ROS 2 support, but you can choose any other odometry sorftware and feed it to Swarm-SLAM on the correct topic. If you still want to give RTAB-Map a try, I suggest looking into those examples: https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch We decoupled the odometry from Swarm-SLAM to make it more general as odometry is often very sensor specific.
Thanks for such a quick response and the link. I'm utterly confused about what is missing.
As far as I understood from the issues on the rtabmap page and GPT, rtabmap requires to have a tf message. If the dataset doesn't have them, in the RGBD example, they add a dummy one. My dataset also doesn't have any tf transforms, it is just a collection of images converted to a ROS2 bag.
From the example you shared, I see that 'kinect' is the frame_id of the nodes participating in rtabmap and they publish a dummy transform to those nodes.
However, two things are unclear to me:
-
Where do they take
openni_rgb_optical_framefrom? -
In your configs, you define similar static transform publishers in the
graco_lidar.launch.py. However, the names have also been recondited for me. Moreover, they're defined in the graco launch file, not in the rtabmap launch file. -
In my case, assuming my odometry launch frame id is
r0_link, from which node ID do I have to publish static transforms to it such that it stops complaining? As a wild guess inX_rgbd.launch.pyI tried:
tf_process = Node(package="tf2_ros",
executable="static_transform_publisher",
arguments="0 0 0 0 0 0 r0_link base_link".split(" "),
parameters=[])
schedule.append(PushLaunchConfigurations())
schedule.append(tf_process)
schedule.append(PopLaunchConfigurations())
but I still get:
[rgbd_odometry-2] [ WARN] (2024-07-31 14:16:14.857) MsgConversion.cpp:1994::getTransform() (can transform r0_link -> ?) Invalid frame ID "" passed to canTransform argument source_frame - in tf2 frame_ids cannot be empty. canTransform returned after 0.0101865 timeout was 0.1. (wait_for_transform=0.100000)
This is an RTAB-Map question, this thread might be useful https://github.com/introlab/rtabmap_ros/issues/35 .
As for which launch file, it does not matter where, as long as I you launch the static_transform_publisher at some point.
Could you post one output of ros2 topic echo /r0/camera/depth/image_rawand ros2 topic echo /r0/camera/color/image_raw? I suspect that the frame_idin the header (http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Header.html) is empty. In which case you will need to fill it with the correct frame (i.e. create a node that subscribe to your topic, fill the missing info, and republish, rerecord the bag with the modified messages).
Indeed, missing a header was a problem, thanks!
There are still 3 things that don't work for me:
- Despite odometry works (also can be seen in the visualization if I add
rtabmap_viznode), I still get warnings like:
I checked the topics with ros2 topic hz <topic_name> and they seem normal:
I also checked the timestamps of all the messages. I record my bag with a standalone python script and assign the same timestamp to both depth, color, and camera info messages:
time_stamp = Clock().now()
for i in tqdm(range(len(img_paths))):
rgb = load_ros_image(str(img_paths[i]), time_stamp)
depth = load_ros_depth(str(depth_paths[i]), time_stamp)
camera_info = load_ros_camera_info(time_stamp)
writer.write('/camera/rgb/image_raw', serialize_message(rgb), time_stamp.nanoseconds)
writer.write('/camera/depth/image_raw', serialize_message(depth), time_stamp.nanoseconds)
writer.write('/camera/info', serialize_message(camera_info), time_stamp.nanoseconds)
time_stamp += frame_duration
and the header e.g. for CameraInfo as:
camera_info = CameraInfo()
camera_info.header.stamp = timestamp.to_msg()
what might be a possible issue here?
-
I went over this issue. I've enabled the logs in the config. However, when a/the bag file(s) are processed, the created folder `results/
_<robot_id> is empty. What might be the reason for this? -
Finally, despite the visualization is enabled in the config, I don't see anything on the screen.
I thought that 2 and 3 are related to the fact that cslam components are not getting odometry, but I checked it with:
and:
May there be something else that the slam components are waiting for but can't get? I currently use cslam_rgbd.launch.py so maybe I need to set something there additionally?
The first one is weird indeed. As this is a RTAB-Map problem, I would look in their issues and forums.
To debug 2 and 3, can you ros2 topic echo a few topics in the Swarm-SLAM pipeline to see where the pipeline breaks. You can generally follow the pipeline by checking what are the subscribers and publishers of a node, and then finding the next node from there.
For example the first node in the RGBD pipeline is the map_manager (with the RGBD_handler), it should publish on keyframe_data and keyframe_odom as it receives messages https://github.com/lajoiepy/cslam/blob/32527430ef271064d468f442a3ab754917abb011/src/front_end/rgbd_handler.cpp#L77