Re: Navigation with Rtabmap and Stereo Camera

Posted by imran05 on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Navigation-with-Rtabmap-and-Stereo-Camera-tp10644p10661.html

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?