Error when using external odometry for RTAB-Map SLAM
Hello,
I have a differential-drive robot that is equipped with a realsense D435 RGBD camera and IMU. I wanted to fuse the sensor readings from the stereo odometry produced from the stereo_odometry node in the rtabmapros package with the IMU that I have using an extended kalman filter using the `robotlocalizationpackage. When I was able to implement this, I placed the produced odometry topic,/odometry/filtered, in the standard rtabmap launch filertabmap.launchby replacing the remaped odom topic in the rtabmap SLAM nodertabmap` (line 410) with the filtered odometry topic:
<remap from="odom" to="/odometry/filtered"/>
When doing this, the filtered odometry is produced fine, however, it seems as though the SLAM node cannot receive neither stereo camera images nor the produced external odometry because no point cloud nor occupancy grid data appears and I consistently receive this error message:
[ WARN] [1679352722.546661048]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz mytopic") 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"). Parameter "approxsync" is false, which means that input topics should have all the exact timestamp for the callback to be called. /rtabmap/rtabmap subscribed to (exact sync):
/odometry/filtered *
/camera/infra1/imagerectraw_relay *
/camera/infra2/imagerectraw_relay *
/camera/infra1/camera_info *
/camera/infra2/camera_info *
/rtabmap/odom_info *
When I run rostopic echo on all of the above topics, they all output some values consistently, so I am not sure why the SLAM node is not receiving the messages. Below, I will display the launch file that I am using to initialize the rtabmap.launch file, the IMU, and the ekf node:
<launch>
<!-- This launch assumes that you have already
started you preferred RGB-D sensor and your IMU.
TF between frame_id and the sensors should already be set too. -->
<arg name="rviz" default="true" />
<arg name="frame_id" default="base_link"/>
<arg name="vis_odom_topic" default="/rtabmap/odom"/>
<arg name="imu_topic" default="/imu/data" />
<arg name="imu_remove_gravitational_acceleration" default="true" />
<arg name="localization" default="false"/>
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg unless="$(arg localization)" name="rtabmap_args" value="--delete_db_on_start"/>
<arg name="frame_id" value="$(arg frame_id)"/>
<arg name="stereo" value="true"/>
<arg name="subscribe_rgbd" value="false"/>
<arg name="subscribe_scan" value="true"/>
<!-- stereo related topics -->
<arg name="left_image_topic" default="/camera/infra1/image_rect_raw" />
<arg name="right_image_topic" default="/camera/infra2/image_rect_raw" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="/camera/infra1/camera_info" />
<arg name="right_camera_info_topic" default="/camera/infra2/camera_info" />
<arg name="rgbd_topic" value="/rgbd_image"/>
<arg name="visual_odometry" value="true"/>
<arg name="queue_size" value="30"/>
<arg name="compressed" value="true"/>
<arg name="rviz" value="$(arg rviz)"/>
<arg if="$(arg rviz)" name="rtabmapviz" value="false"/>
</include>
<!--Launch static transform between base_link and camera_link-->
<node name="base_to_camera_static_publisher" pkg="tf2_ros" type="static_transform_publisher" args="0.2531 0 0.0859 0 0 0 base_link camera_link"/>
<!-- Initializing teleop node-->
<remap from="vel_cmd" to="mega_one/vel_cmd"/>
<node pkg="devbot_teleop" type="teleop_control.py" name="teleop_node" />
<!-- Launching imu node-->
<remap from="bno055" to="mega_two/bno055" />
<include file="$(find bno055_imu_ros)/launch/bno055_imu.launch">
<!-- Set custom parameters if needed
<param name="linear_velocity_stdev" type="double" value="" />
<param name="angular_velocity_stdev" type="double" value="" />
-->
</include>
<!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
<param name="frequency" value="50"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="true"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="$(arg frame_id)"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="$(arg vis_odom_topic)"/>
<param name="imu0" value="$(arg imu_topic)"/>
<!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="odom0_config">[true, true, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="true"/>
<param name="imu0_relative" value="true"/>
<param name="odom0_queue_size" value="5"/>
<param name="imu0_queue_size" value="50"/>
<!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="process_noise_covariance">[0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<!-- The values are ordered as x, y,
z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
Please note that this launch file is being ran from a remote computer and the robot uses a different launch file that launches the stereo_sync node. Just in case it is important, that will also be displayed below:
<launch>
<!-- Launch Stereo Camera Node-->
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="enable_infra1" value="true" />
<arg name="enable_infra2" value="true" />
</include>
<!-- Launch Rosserial Nodes for Both Arduino Mega Boards (57600 Baud Rate)-->
<node ns="mega_one" pkg="rosserial_python" type="serial_node.py" name="serial_node_one" args="/dev/ttyACM0"/>
<node ns="mega_two" pkg="rosserial_python" type="serial_node.py" name="serial_node_two" args="/dev/ttyACM1"/>
<!-- Launch RTAB-Map Stereo Sync Nodelet-->
<arg name="rate" default="5"/>
<arg name="approx_sync" default="false" />
<arg name="stereo_sync" default="true"/>
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
<node if="$(arg stereo_sync)" pkg="nodelet" type="nodelet" name="stereo_sync" args="load rtabmap_ros/stereo_sync nodelet_manager" output="screen">
<param name="compressed_rate" type="double" value="$(arg rate)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<remap from="left/image_rect" to="/camera/infra1/image_rect_raw"/>
<remap from="right/image_rect" to="/camera/infra2/image_rect_raw"/>
<remap from="left/camera_info" to="/camera/infra1/camera_info"/>
<remap from="right/camera_info" to="/camera/infra2/camera_info"/>
<remap from="rgbd_image" to="rgbd_image"/>
</node>
Both the computer and the robot are being ran using the noetic distro of ROS. Any advice on how I could debug this problem would be greatly appreciated!
Asked by Devin1126 on 2023-03-20 18:52:00 UTC
Comments