VladimirYugay
commented
3 months ago 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 scheduleBag 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: 1also, 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)
lajoiepy
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:
resnet18_64.pthto/Swarm-SLAM/install/cslam/share/cslam/models/Here's my rosbag info:
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: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:rtabmap_X_rgbd_odometry.launch.pyto provide odometry to the method:bag_X.launch.py:I'm getting a warning:
My rostopic list looks as:
However, when I start it, I get a warning:
Also, I get:
am I missing something?