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.

[imran05]

Hello,

Sorry for the delayed response. I am currently following your suggestion to detect ground and obstacles on 3D terrain. For this, I am utilizing the rtabmap_util/obstacles_detection nodelet.

However, I am working with a DepthAI camera and using the stereo_inertial_publisher from this link. Unfortunately, I encountered the following error when trying to run the nodelet.

 INFO] [1735196693.334009482]: Initializing nodelet with 8 worker threads.
[ INFO] [1735196693.341195588]: waitForService: Service [/nodelet_manager/load_nodelet] is now available.
 nnPath ,, /home/darlab/dai_ws/src/depthai-ros/depthai_examples/resources/yolov4_tiny_coco_416x416_openvino_2021.4_6shave_bgr.blob
1280 720 32764 1750354912
[ERROR] [1735196693.498634459]: Failed to load nodelet [/obstacles_detection] of type [rtabmap_util/obstacles_detection] even after refreshing the cache: According to the loaded plugin descriptions the class rtabmap_util/obstacles_detection with base class type nodelet::Nodelet does not exist. Declared types are  RobotLocalization/EkfNodelet RobotLocalization/NavSatTransformNodelet RobotLocalization/UkfNodelet SlamGMappingNodelet apriltag_ros/ContinuousDetector depth_image_proc/convert_metric depth_image_proc/crop_foremost depth_image_proc/disparity depth_image_proc/point_cloud_xyz depth_image_proc/point_cloud_xyz_radial depth_image_proc/point_cloud_xyzi depth_image_proc/point_cloud_xyzi_radial depth_image_proc/point_cloud_xyzrgb depth_image_proc/point_cloud_xyzrgb_radial depth_image_proc/register depthai_examples/StereoNodelet depthai_filters/Detection2DOverlay depthai_filters/FeatureTrackerOverlay depthai_filters/Features3D depthai_filters/SegmentationOverlay depthai_filters/SpatialBB depthai_filters/WLSFilter depthai_ros_driver/Camera image_proc/crop_decimate image_proc/crop_nonZero image_proc/crop_non_zero image_proc/debayer image_proc/rectify image_proc/resize image_publisher/image_publisher image_rotate/image_rotate image_view/disparity image_view/image imu_complementary_filter/ComplementaryFilterNodelet imu_filter_madgwick/ImuFilterNodelet laser_filters/scan_to_cloud_filter_chain laser_filters/scan_to_scan_filter_chain nodelet_tutorial_math/Plus pcl/BAGReader pcl/BoundaryEstimation pcl/ConvexHull2D pcl/CropBox pcl/EuclideanClusterExtraction pcl/ExtractIndices pcl/ExtractPolygonalPrismData pcl/FPFHEstimation pcl/FPFHEstimationOMP pcl/MomentInvariantsEstimation pcl/MovingLeastSquares pcl/NodeletDEMUX pcl/NodeletMUX pcl/NormalEstimation pcl/NormalEstimationOMP pcl/NormalEstimationTBB pcl/PCDReader pcl/PCDWriter pcl/PFHEstimation pcl/PassThrough pcl/PointCloudConcatenateDataSynchronizer pcl/PointCloudConcatenateFieldsSynchronizer pcl/PrincipalCurvaturesEstimation pcl/ProjectInliers pcl/RadiusOutlierRemoval pcl/SACSegmentation pcl/SACSegmentationFromNormals pcl/SHOTEstimation pcl/SHOTEstimationOMP pcl/SegmentDifferences pcl/StatisticalOutlierRemoval pcl/VFHEstimation pcl/VoxelGrid phidgets_analog_inputs/PhidgetsAnalogInputsNodelet phidgets_digital_inputs/PhidgetsDigitalInputsNodelet phidgets_digital_outputs/PhidgetsDigitalOutputsNodelet realsense2_camera/RealSenseNodeFactory stereo_image_proc/disparity stereo_image_proc/point_cloud2 velodyne_driver/DriverNodelet velodyne_laserscan/LaserScanNodelet velodyne_pointcloud/RingColorsNodelet velodyne_pointcloud/TransformNodelet
[ERROR] [1735196693.498746568]: The error before refreshing the cache was: According to the loaded plugin descriptions the class rtabmap_util/obstacles_detection with base class type nodelet::Nodelet does not exist. Declared types are  RobotLocalization/EkfNodelet RobotLocalization/NavSatTransformNodelet RobotLocalization/UkfNodelet SlamGMappingNodelet apriltag_ros/ContinuousDetector depth_image_proc/convert_metric depth_image_proc/crop_foremost depth_image_proc/disparity depth_image_proc/point_cloud_xyz depth_image_proc/point_cloud_xyz_radial depth_image_proc/point_cloud_xyzi depth_image_proc/point_cloud_xyzi_radial depth_image_proc/point_cloud_xyzrgb depth_image_proc/point_cloud_xyzrgb_radial depth_image_proc/register depthai_examples/StereoNodelet depthai_filters/Detection2DOverlay depthai_filters/FeatureTrackerOverlay depthai_filters/Features3D depthai_filters/SegmentationOverlay depthai_filters/SpatialBB depthai_filters/WLSFilter depthai_ros_driver/Camera image_proc/crop_decimate image_proc/crop_nonZero image_proc/crop_non_zero image_proc/debayer image_proc/rectify image_proc/resize image_publisher/image_publisher image_rotate/image_rotate image_view/disparity image_view/image imu_complementary_filter/ComplementaryFilterNodelet imu_filter_madgwick/ImuFilterNodelet laser_filters/scan_to_cloud_filter_chain laser_filters/scan_to_scan_filter_chain nodelet_tutorial_math/Plus pcl/BAGReader pcl/BoundaryEstimation pcl/ConvexHull2D pcl/CropBox pcl/EuclideanClusterExtraction pcl/ExtractIndices pcl/ExtractPolygonalPrismData pcl/FPFHEstimation pcl/FPFHEstimationOMP pcl/MomentInvariantsEstimation pcl/MovingLeastSquares pcl/NodeletDEMUX pcl/NodeletMUX pcl/NormalEstimation pcl/NormalEstimationOMP pcl/NormalEstimationTBB pcl/PCDReader pcl/PCDWriter pcl/PFHEstimation pcl/PassThrough pcl/PointCloudConcatenateDataSynchronizer pcl/PointCloudConcatenateFieldsSynchronizer pcl/PrincipalCurvaturesEstimation pcl/ProjectInliers pcl/RadiusOutlierRemoval pcl/SACSegmentation pcl/SACSegmentationFromNormals pcl/SHOTEstimation pcl/SHOTEstimationOMP pcl/SegmentDifferences pcl/StatisticalOutlierRemoval pcl/VFHEstimation pcl/VoxelGrid phidgets_analog_inputs/PhidgetsAnalogInputsNodelet phidgets_digital_inputs/PhidgetsDigitalInputsNodelet phidgets_digital_outputs/PhidgetsDigitalOutputsNodelet realsense2_camera/RealSenseNodeFactory stereo_image_proc/disparity stereo_image_proc/point_cloud2 velodyne_driver/DriverNodelet velodyne_laserscan/LaserScanNodelet velodyne_pointcloud/RingColorsNodelet velodyne_pointcloud/TransformNodelet
[FATAL] [1735196693.499059228]: Failed to load nodelet '/obstacles_detection` of type `rtabmap_util/obstacles_detection` to manager `nodelet_manager'
[ WARN] [1735196693.551590281]: Joint state with name: "ffl_wheel_joint" was received but not found in URDF
[obstacles_detection-5] process has died [pid 122654, exit code 255, cmd /opt/ros/noetic/lib/nodelet/nodelet load rtabmap_util/obstacles_detection nodelet_manager cloud:=/stereo_inertial_publisher/stereo/points obstacles:=/obstacles_cloud ground:=/ground_cloud __name:=obstacles_detection __log:=/home/darlab/.ros/log/de12d66e-c356-11ef-9ab5-cdbbfe9492ca/obstacles_detection-5.log].
log file: /home/darlab/.ros/log/de12d66e-c356-11ef-9ab5-cdbbfe9492ca/obstacles_detection-5*.log

Could you please guide me on how to correctly run this nodelet? Alternatively, is it possible to run the obstacles_detection functionality without relying on a nodelet?

Thank you for your help!

[matlabbe]

The standalone node is not available for that nodelet. Based on the log, all rtabmap nodelets are missing. Make sure to source your catkin setup.bash if you built rtabmap locally. Also, did you mean this example?

[imran05]

Yes, I a talking about the same example but in my case the i am not using the disparity2cloud from rtabmap util since my point clouds are generted from a depthai camera using stereo_inertial_node.launch . So, for only the object detection node i am using the rtabmap_util/objection_detection notelet.

Moreover, I have a two separate workspace:
1) dai_ws: for the depthai camera in which i am running the stereo inertial node with depth aligned is false.
2) rtabmap_ws : for rtabmap slama dn here i am running only the

<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_util/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="min_cluster_size" type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.0"/>
</node>

I properly source the both environment correctly even though the rtabmap slam node is working in the same environment both not the objection detction nodelet.
 The error are like belows.
Before the objection detection the stereo inertial nodes works fine but after i run the objection dection i got this error

ERROR] [1735279172.722923331]: Failed to load nodelet [/obstacles_detection] of type [rtabmap_util/obstacles_detection] even after refreshing the cache: According to the loaded plugin descriptions the class rtabmap_util/obstacles_detection with base class type nodelet::Nodelet does not exist. Declared types are  RobotLocalization/EkfNodelet RobotLocalization/NavSatTransformNodelet RobotLocalization/UkfNodelet SlamGMappingNodelet apriltag_ros/ContinuousDetector depth_image_proc/convert_metric depth_image_proc/crop_foremost depth_image_proc/disparity depth_image_proc/point_cloud_xyz depth_image_proc/point_cloud_xyz_radial depth_image_proc/point_cloud_xyzi depth_image_proc/point_cloud_xyzi_radial depth_image_proc/point_cloud_xyzrgb depth_image_proc/point_cloud_xyzrgb_radial depth_image_proc/register depthai_examples/StereoNodelet depthai_filters/Detection2DOverlay depthai_filters/FeatureTrackerOverlay depthai_filters/Features3D depthai_filters/SegmentationOverlay depthai_filters/SpatialBB depthai_filters/WLSFilter depthai_ros_driver/Camera image_proc/crop_decimate image_proc/crop_nonZero image_proc/crop_non_zero image_proc/debayer image_proc/rectify image_proc/resize image_publisher/image_publisher image_rotate/image_rotate image_view/disparity image_view/image imu_complementary_filter/ComplementaryFilterNodelet imu_filter_madgwick/ImuFilterNodelet laser_filters/scan_to_cloud_filter_chain laser_filters/scan_to_scan_filter_chain nodelet_tutorial_math/Plus pcl/BAGReader pcl/BoundaryEstimation pcl/ConvexHull2D pcl/CropBox pcl/EuclideanClusterExtraction pcl/ExtractIndices pcl/ExtractPolygonalPrismData pcl/FPFHEstimation pcl/FPFHEstimationOMP pcl/MomentInvariantsEstimation pcl/MovingLeastSquares pcl/NodeletDEMUX pcl/NodeletMUX pcl/NormalEstimation pcl/NormalEstimationOMP pcl/NormalEstimationTBB pcl/PCDReader pcl/PCDWriter pcl/PFHEstimation pcl/PassThrough pcl/PointCloudConcatenateDataSynchronizer pcl/PointCloudConcatenateFieldsSynchronizer pcl/PrincipalCurvaturesEstimation pcl/ProjectInliers pcl/RadiusOutlierRemoval pcl/SACSegmentation pcl/SACSegmentationFromNormals pcl/SHOTEstimation pcl/SHOTEstimationOMP pcl/SegmentDifferences pcl/StatisticalOutlierRemoval pcl/VFHEstimation pcl/VoxelGrid phidgets_analog_inputs/PhidgetsAnalogInputsNodelet phidgets_digital_inputs/PhidgetsDigitalInputsNodelet phidgets_digital_outputs/PhidgetsDigitalOutputsNodelet realsense2_camera/RealSenseNodeFactory stereo_image_proc/disparity stereo_image_proc/point_cloud2 velodyne_driver/DriverNodelet velodyne_laserscan/LaserScanNodelet velodyne_pointcloud/RingColorsNodelet velodyne_pointcloud/TransformNodelet
[ERROR] [1735279172.723172848]: The error before refreshing the cache was: According to the loaded plugin descriptions the class rtabmap_util/obstacles_detection with base class type nodelet::Nodelet does not exist. Declared types are  RobotLocalization/EkfNodelet RobotLocalization/NavSatTransformNodelet RobotLocalization/UkfNodelet SlamGMappingNodelet apriltag_ros/ContinuousDetector depth_image_proc/convert_metric depth_image_proc/crop_foremost depth_image_proc/disparity depth_image_proc/point_cloud_xyz depth_image_proc/point_cloud_xyz_radial depth_image_proc/point_cloud_xyzi depth_image_proc/point_cloud_xyzi_radial depth_image_proc/point_cloud_xyzrgb depth_image_proc/point_cloud_xyzrgb_radial depth_image_proc/register depthai_examples/StereoNodelet depthai_filters/Detection2DOverlay depthai_filters/FeatureTrackerOverlay depthai_filters/Features3D depthai_filters/SegmentationOverlay depthai_filters/SpatialBB depthai_filters/WLSFilter depthai_ros_driver/Camera image_proc/crop_decimate image_proc/crop_nonZero image_proc/crop_non_zero image_proc/debayer image_proc/rectify image_proc/resize image_publisher/image_publisher image_rotate/image_rotate image_view/disparity image_view/image imu_complementary_filter/ComplementaryFilterNodelet imu_filter_madgwick/ImuFilterNodelet laser_filters/scan_to_cloud_filter_chain laser_filters/scan_to_scan_filter_chain nodelet_tutorial_math/Plus pcl/BAGReader pcl/BoundaryEstimation pcl/ConvexHull2D pcl/CropBox pcl/EuclideanClusterExtraction pcl/ExtractIndices pcl/ExtractPolygonalPrismData pcl/FPFHEstimation pcl/FPFHEstimationOMP pcl/MomentInvariantsEstimation pcl/MovingLeastSquares pcl/NodeletDEMUX pcl/NodeletMUX pcl/NormalEstimation pcl/NormalEstimationOMP pcl/NormalEstimationTBB pcl/PCDReader pcl/PCDWriter pcl/PFHEstimation pcl/PassThrough pcl/PointCloudConcatenateDataSynchronizer pcl/PointCloudConcatenateFieldsSynchronizer pcl/PrincipalCurvaturesEstimation pcl/ProjectInliers pcl/RadiusOutlierRemoval pcl/SACSegmentation pcl/SACSegmentationFromNormals pcl/SHOTEstimation pcl/SHOTEstimationOMP pcl/SegmentDifferences pcl/StatisticalOutlierRemoval pcl/VFHEstimation pcl/VoxelGrid phidgets_analog_inputs/PhidgetsAnalogInputsNodelet phidgets_digital_inputs/PhidgetsDigitalInputsNodelet phidgets_digital_outputs/PhidgetsDigitalOutputsNodelet realsense2_camera/RealSenseNodeFactory stereo_image_proc/disparity stereo_image_proc/point_cloud2 velodyne_driver/DriverNodelet velodyne_laserscan/LaserScanNodelet velodyne_pointcloud/RingColorsNodelet velodyne_pointcloud/TransformNodelet



and in objection detction node terminal i am getting error:
[FATAL] [1735279172.723533399]: Failed to load nodelet '/obstacles_detection` of type `rtabmap_util/obstacles_detection` to manager `nodelet_manager'
[obstacles_detection-1] process has died [pid 8112, exit code 255, cmd /opt/ros/noetic/lib/nodelet/nodelet load rtabmap_util/obstacles_detection nodelet_manager cloud:=/stereo_inertial_publisher/stereo/points obstacles:=/obstacles_cloud ground:=/ground_cloud __name:=obstacles_detection __log:=/home/darlab/.ros/log/afdf4cfc-c417-11ef-915d-0da0808d7855/obstacles_detection-1.log].
log file: /home/darlab/.ros/log/afdf4cfc-c417-11ef-915d-0da0808d7855/obstacles_detection-1*.log

[matlabbe]

Does this work?

roscd rtabmap_util

then this?

roscore
rosrun nodelet nodelet standalone rtabmap_util/obstacles_detection
type is rtabmap_util/obstacles_detection

In your launch file, is there a nodelet manager? You can also do:

<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="standalone rtabmap_util/obstacles_detection">
      <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="min_cluster_size" type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.0"/>
</node>

In the example, we used disparity2cloud to downsample the point cloud because there would be too many points to process, slowing down the segmentation.

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

However with latest obstacles_detection implementation, the point cloud would be downsampled to 5 cm voxel by default (Grid/CellSize=0.05 and Grid/PreVoxelFiltering=true), so the overhead may not be too much to use the full resolution point cloud.

[imran05]

Thank You, with standalone the obstacle detection node starts working but my rtabmap slam node is crashing now even without it. I tried to figure it out but the solutions #8 and this  i found didn't wok for me.

I am getting below errors now:
[FATAL] (2024-12-30 14:40:27.819) Features2d.cpp:2513::generateDescriptorsImpl() Condition ((int)keypoints.size() == descriptors_.rows) not met! [keypoints=600 descriptors=0]
[FATAL] [1735537227.820026726]: [FATAL] (2024-12-30 14:40:27.819) Features2d.cpp:2513::generateDescriptorsImpl() Condition ((int)keypoints.size() == descriptors_.rows) not met! [keypoints=600 descriptors=0]
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2024-12-30 14:40:27.819) Features2d.cpp:2513::generateDescriptorsImpl() Condition ((int)keypoints.size() == descriptors_.rows) not met! [keypoints=600 descriptors=0]
[rtabmap/stereo_odometry-1] process has died [pid 28663, exit code -6, cmd /home/darlab/rtabmap_ws/devel/lib/rtabmap_odom/stereo_odometry left/image_rect:=/stereo_inertial_publisher/left/image_rect right/image_rect:=/stereo_inertial_publisher/right/image_rect left/camera_info:=/stereo_inertial_publisher/right/camera_info right/camera_info:=/stereo_inertial_publisher/left/camera_info rgbd_image:=rgbd_image_relay odom:=odom imu:=/stereo_inertial_publisher/imu/data __name:=stereo_odometry __log:=/home/darlab/.ros/log/4a8cc3a8-c66e-11ef-bcc0-8146ab82e6d5/rtabmap-stereo_odometry-1.log].
log file: /home/darlab/.ros/log/4a8cc3a8-c66e-11ef-bcc0-8146ab82e6d5/rtabmap-stereo_odometry-1*.log


and sometimes the below one:

FATAL] (2024-12-30 14:40:59.982) util3d_mapping.cpp:826::rayTrace() Condition (end.x >= 0 && end.x < grid.cols) not met! [end.x=-2 grid.cols=200]
[FATAL] [1735537259.982265532]: [FATAL] (2024-12-30 14:40:59.982) util3d_mapping.cpp:826::rayTrace() Condition (end.x >= 0 && end.x < grid.cols) not met! [end.x=-2 grid.cols=200]
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2024-12-30 14:40:59.982) util3d_mapping.cpp:826::rayTrace() Condition (end.x >= 0 && end.x < grid.cols) not met! [end.x=-2 grid.cols=200]
[ INFO] [1735537260.049045844]: Odom: quality=459, std dev=0.019702m|0.038960rad, update time=0.096918s
[ INFO] [1735537260.178868459]: Odom: quality=449, std dev=0.023083m|0.043116rad, update time=0.038624s
[ INFO] [1735537260.307225595]: Odom: quality=447, std dev=0.027751m|0.046331rad, update time=0.075144s
[ INFO] [1735537260.401837107]: Odom: quality=448, std dev=0.027788m|0.046331rad, update time=0.068712s
[ INFO] [1735537260.481651984]: Odom: quality=444, std dev=0.030482m|0.044810rad, update time=0.048171s
[ INFO] [1735537260.603218810]: Odom: quality=439, std dev=0.031161m|0.048989rad, update time=0.067083s
[rtabmap/rtabmap-2] process has died [pid 29139, exit code -6, cmd /home/darlab/rtabmap_ws/devel/lib/rtabmap_slam/rtabmap move_base:=/move_base rgb/image:=/camera/rgb/image_rect_color depth/image:=/camera/depth_registered/image_raw rgb/camera_info:=/camera/rgb/camera_info rgbd_image:=rgbd_image_relay left/image_rect:=/stereo_inertial_publisher/left/image_rect right/image_rect:=/stereo_inertial_publisher/right/image_rect left/camera_info:=/stereo_inertial_publisher/right/camera_info right/camera_info:=/stereo_inertial_publisher/left/camera_info scan:=/scan scan_cloud:=/scan_cloud scan_descriptor:=/scan_descriptor user_data:=/user_data user_data_async:=/user_data_async gps/fix:=/gps/fix tag_detections:=/tag_detections fiducial_transforms:=/fiducial_transforms odom:=odom imu:=/stereo_inertial_publisher/imu/data __name:=rtabmap __log:=/home/darlab/.ros/log/4a8cc3a8-c66e-11ef-bcc0-8146ab82e6d5/rtabmap-rtabmap-2.log].
log file: /home/darlab/.ros/log/4a8cc3a8-c66e-11ef-bcc0-8146ab82e6d5/rtabmap-rtabmap-2*.log


Moreover, the standalone version is working fine, i can run the rtabmap with the previous databases.

[imran05]

Sorry for the above reply, that problem is the rtabmap.ini file that i am giving to rtabmap has some problems so deleted that filed and then again set the parameters from standalone version solved that problem but withen ia m running the obbstacle detction node , iam gwetting error :

[ WARN] (2024-12-30 17:45:28.841) util3d_filtering.cpp:697::voxelizeImpl() Leaf size is too small for the input dataset. Integer indices would overflow. We will split space to be able to voxelize (lvl=0 cloud=322862 min=[-66.547523 -73.875565 -39.656750] max=[-1.468997 70.267570 38.708424] voxel=0.050000).

Do you ahev any idea how i resolve this, as i am using ros noetic so my voxel is already down sampled as you mentioned

[matlabbe]

This is just a warning. If the cloud is already voxelized, your could add to obstacles_detection node:

<param name="Grid/PreVoxelFiltering" value="false"/>

 
However, if you want to keep the voxel filter, you can set Grid/RangeMax to something like:

<param name="Grid/RangeMax" type="double" value="30.0"/>

[imran05]

Thank you for the explanation.

I have a one more question:

I am currently mapping an outdoor orchard environment using stereo odometry. Is there a way to record the positions of trees as landmarks in RTAB-Map? My goal is to enable the robot to spray only when there is a tree in front of the spraying nozzles during autonomous navigation.

Specifically, I would like to know if it is possible for the system to identify and store tree locations in the map during mapping and subsequently use this information to trigger spraying at the appropriate locations. If this can be achieved, could you suggest the best approach to implement it?