Navigation with Rtabmap and Stereo Camera

[imran05]

Hello,

I am using the OAK-D-PRO-POE camera with a stereo sensor and successfully generated a 2D/3D map following the Stereo Handheld Mapping tutorial on the RTAB-Map ROS wiki. My goal is to navigate my robot autonomously using this map.

To achieve this, I am working with the move_base package and following the steps in the Stereo Outdoor Navigation tutorial. However, I am uncertain whether I need to convert the point cloud data to laser scan format for the navigation stack. Several sources suggest this conversion is necessary for proper operation with move_base. Currently, when I run my move_base launch file, I encounter the following error:

pocess[planner/move_base-1]: started with pid [51922]
[ WARN] [1730784550.229903768]: global_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided
[ INFO] [1730784550.232415848]: global_costmap: Using plugin "static_layer"
[ INFO] [1730784550.245615792]: Requesting the map...
[ INFO] [1730784550.854107903]: Resizing costmap to 421 X 421 at 0.050000 m/pix
[ INFO] [1730784550.953536180]: Received a 421 X 421 map at 0.050000 m/pix
[ INFO] [1730784550.961638562]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1730784550.970935649]:     Subscribed to Topics:
[ INFO] [1730784550.982091918]: global_costmap: Using plugin "inflation_layer"
[ WARN] [1730784551.058547847]: local_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided
[ INFO] [1730784551.060172258]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1730784551.070438626]:     Subscribed to Topics: point_cloud_sensor
[ INFO] [1730784551.099451642]: local_costmap: Using plugin "inflation_layer"
[ INFO] [1730784551.167750201]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1730784551.179935695]: Sim period is set to 0.05
[ WARN] [1730784553.146037583]: The openni_points observation buffer has not been updated for 2.06 seconds, and it should be updated every 0.50 seconds.
[ INFO] [1730784553.161125806]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1730784553.168741668]: Recovery behavior will clear layer 'obstacles'
[ WARN] [1730784553.646013966]: The openni_points observation buffer has not been updated for 2.56 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1730784554.146029932]: The openni_points observation buffer has not been updated for 3.06 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1730784554.646104295]: The openni_points observation buffer has not been updated for 3.56 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1730784555.150279887]: The openni_points observation buffer has not been updated for 4.06 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1730784555.646011850]: The openni_points observation buffer has not been updated for 4.56 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1730784556.146003108]: The openni_points observation buffer has not been updated for 5.06 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1730784556.646024576]: The openni_points observation buffer has not been updated for 5.56 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1730784557.146082286]: The openni_points observation buffer has not been updated for 6.06 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1730784557.646060362]: The openni_points observation buffer has not been updated for 6.56 seconds, and it should be updated every 0.50 seconds.

Could you advise on whether point cloud data must be converted to laser scan format, and if so, recommend an approach to make this integration work with move_base?

Thank you!

[matlabbe]

Hi,

One option is to use http://wiki.ros.org/depthimage_to_laserscan and use an obstacle layer in move_base with the generated topic. Refer to http://wiki.ros.org/navigation/Tutorials/RobotSetup ("laser_scan_sensor")

If you want to filter all obstacles over a the floor (and if the camera is tilted), you can use http://wiki.ros.org/pointcloud_to_laserscan (from a point cloud generated from the depth image of your camera).

If you need to detect ground versus obstacles on a 3D terrain, you can follow this example http://wiki.ros.org/rtabmap_ros/Tutorials/StereoOutdoorNavigation or this one (and corresponding costmap settings, not that this config was used in this paper, see Figure 6 for example)

For your error, it seems openni_points topic is not published/generated.

cheers,
Mathieu

[imran05]

Thank you for your previous reply. I followed your instructions and referred to the https://wiki.ros.org/rtabmap_ros/Tutorials/StereoOutdoorNavigation. However, I encountered a couple of issues that I need clarification on.

"1: planner_cloud" Source: I am unsure where the planner_cloud is coming from in the code. Could you please provide more details on this?

2: Point Cloud Topic Mapping: The tutorial uses a built-in point_cloud_xyz node, but in my case, the point cloud is already being published from the stereo_inertial_node on the topic /stereo_inertial_publisher/stereo/depth. I used the obstacle_detection node in my navigation launch file, but I am still encountering the previous error. Could you please guide me on how to resolve this error and clarify which topic I need to remap for obstacle detection?

Here are the steps I followed:

roslaunch hada_bringup hada_robot_base.launch

roslaunch depthai_examples stereo_inertial_node.launch depth_aligned:=false

rosrun imu_filter_madgwick imu_filter_node \
    imu/data_raw:=/stereo_inertial_publisher/imu \
    imu/data:=/stereo_inertial_publisher/imu/data \
    _use_mag:=false \
    _publish_tf:=false

roslaunch rtabmap_launch rtabmap_2.launch \
    localization:=true \
    stereo:=true \
    left_image_topic:=/stereo_inertial_publisher/left/image_rect \
    right_image_topic:=/stereo_inertial_publisher/right/image_rect \
    left_camera_info_topic:=/stereo_inertial_publisher/right/camera_info \
    right_camera_info_topic:=/stereo_inertial_publisher/left/camera_info \
    imu_topic:=/stereo_inertial_publisher/imu/data \
    frame_id:=base_footprint \
    approx_sync:=true \
    approx_sync_max_interval:=0.001 \
    wait_imu_to_init:=true

roslaunch hada_navigation hada_navigation_2.launch

I am using the following navigation launch file:

<launch>
    <launch>
    <!-- ROS navigation stack move_base -->
    <group ns="planner">
        <!-- Remap topics for move_base -->
        <remap from="openni_points" to="/planner_cloud"/>
        <remap from="base_scan" to="/base_scan"/>
        <remap from="map" to="/rtabmap/grid_map"/>
        <remap from="move_base_simple/goal" to="/planner_goal"/>

        <!-- Launch move_base node -->
        <node pkg="move_base" type="move_base" name="move_base" respawn="false" output="screen">
            <rosparam file="$(find hada_navigation)/config/costmap_common_params_1.yaml" command="load" ns="global_costmap"/>
            <rosparam file="$(find hada_navigation)/config/costmap_common_params_1.yaml" command="load" ns="local_costmap"/>
            <rosparam file="$(find hada_navigation)/config/local_costmap_params_1.yaml" command="load"/>
            <rosparam file="$(find hada_navigation)/config/global_costmap_params_1.yaml" command="load" ns="global_costmap"/>
            <rosparam file="$(find hada_navigation)/config/base_local_planner_params_1.yaml" command="load"/>
        </node>

        <!-- Set command velocity parameter -->
        <param name="cmd_vel" value="10"/>
    </group>

    <!-- Create point cloud for the planner -->
    <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>
        <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_util/point_cloud_xyz stereo_nodelet">
            <remap from="disparity/image" to="disparity"/>
            <remap from="disparity/camera_info" to="right/camera_info_throttle"/>
            <remap from="cloud" to="cloudXYZ"/>

            <param name="voxel_size" type="double" value="0.05"/>
            <param name="decimation" type="int" value="4"/>
            <param name="max_depth" type="double" value="4"/>
        </node>

    <!-- Obstacle detection node -->
        <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_util/obstacles_detection stereo_nodelet">
            <remap from="cloud" to="cloudXYZ"/>
            <remap from="obstacles" to="/planner_cloud"/>

            <param name="frame_id" type="string" value="base_footprint"/>
            <param name="map_frame_id" type="string" value="map"/>
            <param name="wait_for_transform" type="bool" value="true"/>
            <param name="Grid/MinClusterSize" type="int" value="20"/>
            <param name="Grid/MaxObstacleHeight" type="double" value="0.0"/>
        </node>
</launch>

I am using the same costmap parameters as provided in the tutorial, and I have also tried a different set of parameters without success. Could you please assist me in troubleshooting the error and confirm the correct topic remapping needed for the obstacle detection node?

[matlabbe]

1. The "planner_cloud" is coming from rtabmap_util/obstacles_detection nodelet (remap from the obstacles cloud), which segments the point cloud from rtabmap_util/point_cloud_xyz, which is generated from the disparity image of the camera.

You can use `rqt_graph` to better see how topics are linked.

2. The topic "/stereo_inertial_publisher/stereo/depth" sounds more like a depth image than a point cloud. point_cloud_xyz node can convert the depth image into a point cloud, than you can feed that point cloud to obstacles_detection.

You can also do `rosnode info ...` to see if the topic remaps worked or not, or to just know which topics the nodes are subscribed to and topics that are published.

[imran05]

Sorry actually i mean that the point clouds is from stereo_inertial_node that is "/stereo_inertial_publisher/stereo/depth". So in rtabmap_util/obstacle detection i used:

<remap from="cloud" to="/stereo_inertial_publisher/stereo/points"/>

 .
From the rqt_graph, i am clearly see that /planner/move_base subscribed to rtabmap/grid_map and its publishing the topic cmd_vel but i am not getting any data on cmd_vel. Attached is the rqt_graph.png.
Can you please have a look to this and telll me where i am doing a mistake?. Below that i just used the rtabmap.launch with localization:=true. nag my navigation file is bnelow:

<launch>
    <!-- ROS navigation stack move_base -->
    <group ns="planner">
        <!-- Remap topics for move_base -->
        <remap from="openni_points" to="/planner_cloud"/>
        <remap from="map" to="/rtabmap/grid_map"/>
        <remap from="move_base_simple/goal" to="/planner_goal"/>

        <!-- Launch move_base node -->
        <node pkg="move_base" type="move_base" name="move_base" respawn="false" output="screen">
            <rosparam file="$(find hada_navigation)/config/costmap_common_params_1.yaml" command="load" ns="global_costmap"/>
            <rosparam file="$(find hada_navigation)/config/costmap_common_params_1.yaml" command="load" ns="local_costmap"/>
            <rosparam file="$(find hada_navigation)/config/local_costmap_params_1.yaml" command="load"/>
            <rosparam file="$(find hada_navigation)/config/global_costmap_params_1.yaml" command="load" ns="global_costmap"/>
            <rosparam file="$(find hada_navigation)/config/base_local_planner_params_1.yaml" command="load"/>

            <remap from="/planner/cmd_vel" to="/cmd_vel"/>
            <remap from="/planner/odom" to="/rtabmap/odom"/>
             
        </node>

        <!-- Set command velocity parameter -->
        <param name="cmd_vel" value="10"/>
    </group>


    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load /obstacles_detection stereo_nodelet">
        <remap from="cloud" to="/stereo_inertial_publisher/stereo/points"/>
        <remap from="obstacles" to="/planner_cloud"/>
        <param name="frame_id" type="string" value="base_footprint"/>
        <param name="map_frame_id" type="string" value="map"/>
        <param name="wait_for_transform" type="bool" value="true"/>
        <param name="Grid/MinClusterSize" type="int" value="20"/>
        <param name="Grid/MaxObstacleHeight" type="double" value="0.0"/>
    </node>
</launch>

rqt_Graph: rosgraph.png

[matlabbe]

Look at the log messages from move_base when you send a goal. It should acknowledge it received the goal and if it fails, it should show why.

[imran05]

Actually my move_base is continously showing me the below warning:

[ WARN] [1731993526.091251285]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 37.59 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993526.591260567]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 38.09 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993527.091547784]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 38.59 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993527.591321223]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 39.09 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993528.091315314]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 39.59 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993528.591379434]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 40.09 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993529.091318137]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 40.59 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993529.591272867]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 41.09 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993530.091351618]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 41.59 seconds, and it should be updated every 0.50 seconds.
[ WARN] [1731993530.591412345]: The /stereo_inertial_publisher/stereo/points observation buffer has not been updated for 42.09 seconds, and it should be updated every 0.50 seconds.

And when i checked the /stereo_inertial_publisher/stereo/points, the pointclouds are publishing.


I also have some more questions to verify:
1) In the example file: their ios a node used abtr_priority is this important to do this step are we can leave it as our main topic we are using like "/cmd_vel" in my case.
2)What is the use of "data_throttle" in the example file.

[matlabbe]

If /stereo_inertial_publisher/stereo/points is published and move_base is warning like that, I cannot really help more, except check if frame_ids and stamps are correctly set in the topics.

For abtr_priority, you can ignore it, I think it was used with an arbitration node to give priority from some cmd_vel over others. For example, having a teleop controller sending also cmd_vel, then having higher priority than move_base to send cmd_vel to the robot.

The data_throttle was used to avoid producing point clouds at 30 Hz if the costmap is updated only at 5 or 10 Hz.

cheers,
Mathieu

[imran05]

Thank You For your response.

I apologize for the confusion earlier. The error was occurring due to an incorrect remapping of the topic in the /planner. After I corrected the remapping, the error was resolved. However, I am now encountering a new issue: when I send a goal from RViz, the robot receives the cmd_vel command but moves very slowly.


Additionally, I am seeing the following warnings in the move_base terminal:

[ INFO] [1732606086.849031416]: global_costmap: Using plugin "static_layer"
[ INFO] [1732606086.865037291]: Requesting the map...
[ INFO] [1732606087.180699676]: Resizing costmap to 421 X 421 at 0.050000 m/pix
[ INFO] [1732606087.280328884]: Received a 421 X 421 map at 0.050000 m/pix
[ INFO] [1732606087.291234878]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1732606087.300996839]:     Subscribed to Topics: 
[ INFO] [1732606087.314020466]: global_costmap: Using plugin "inflation_layer"
[ WARN] [1732606087.416201429]: local_costmap: Parameter "plugins" not provided, loading pre-Hydro parameters
[ INFO] [1732606087.429635252]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1732606087.435582336]:     Subscribed to Topics: point_cloud_sensor
[ INFO] [1732606087.469182128]: local_costmap: Using plugin "inflation_layer"
[ INFO] [1732606087.544075587]: Created local_planner dwa_local_planner/DWAPlannerROS
[ INFO] [1732606087.550245564]: Sim period is set to 0.10
[ WARN] [1732606088.528829741]: The openni_points observation buffer has not been updated for 1.08 seconds, and it should be updated every 0.50 seconds.
[ INFO] [1732606088.547545547]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1732606088.561968470]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1732606088.721122898]: odom received!
[ INFO] [1732606118.409030525]: Got new plan
[ WARN] [1732606119.245870677]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1370 seconds
[ WARN] [1732606119.775476997]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1667 seconds
[ WARN] [1732606119.829817448]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1210 seconds
[ WARN] [1732606120.228652360]: DWA planner failed to produce path.
[ WARN] [1732606120.236451311]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1276 seconds
[ INFO] [1732606120.236639892]: Got new plan
[ WARN] [1732606120.334149797]: DWA planner failed to produce path.
[ INFO] [1732606120.409177037]: Got new plan
[ WARN] [1732606120.452401446]: DWA planner failed to produce path.
[ INFO] [1732606120.509990015]: Got new plan
[ WARN] [1732606120.552900047]: DWA planner failed to produce path.
[ INFO] [1732606120.608923694]: Got new plan
[ WARN] [1732606120.710232212]: Control loop missed its desired rate of 10.0000Hz... the loop actually took 0.1013 seconds

and sometimes i am slo getting the warning:

Clearing both costmap outside the square (3.0m) large centred on the robot.
Rotate recovery behaviour started

.

Could you please advise on what might be causing this slow response and how to address it?

Thank you for your help!

[matlabbe]

To debug navigation, add local costmap, global costmap, global planner path, local planner path in rviz, echo /cmd_vel published to make sure there is not another node publishing 0 cmd_vel at the same time. For the speed, you can change the move_base controller speed/acceleration parameters. Use rqt_reconfigure to do so for convenience to see the changes live.