Stereo Navigation and Mapping

[Markus]

Hey there,
First of all, I want to thank Mathieu Labbé for this great system.

I am trying to use rtabmap for navigation and mapping with pioneer3at combined with a stereo-camera system (bumblebee2).

The mapping is working great and now I try to go for the next step and have also the navigation part included, but with no prior knowledge about the environment. Actually, what I try is SLAM + Navigation. To start of, it would be enough to send navGoals2D via the RVIZ UI and the robot make its way to the goal.

I followed the az3_mapping_robot_stereo_nav.launch as provided by the corresponding ROS wiki page, but for some reasons I get the following error every 5 seconds:

"[ WARN] [1441225422.168764378]: Waiting on transform from base_footprint to map to become available before running costmap, tf error: . canTransform returned after 0.100647 timeout was 0.1.. canTransform returned after 0.100674 ..."

Has anyone encountered a similar behavior?

I start the bumblee-driver on the robot, as well as rosaria to communicate with the platform.
The rest is started on remote-oc, including roscore.

What parts am I missing to fulfill the task?

Thank you so much. I appreciate any help.

I attached the rqt_graph. It seems that the planner and rosaria just float around...

Here is my launch-file:

<launch>


<arg name="rviz" default="true" />
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="-$(arg pi/2) 0 -$(arg pi/2)" />
<arg name="optical_translate" value="-0.10 0 0.72" />


<group ns="planner">
    <remap from="openni_points" to="/planner_cloud"/>
    <remap from="base_scan" to="/base_scan"/>
    <remap from="map" to="/map"/>
    <remap from="move_base_simple/goal" to="/planner_goal"/>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/local_costmap_params_3d.yaml" command="load" />
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/global_costmap_params.yaml" command="load" />
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/base_local_planner_params.yaml" command="load" />
    </node>

</group>


<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
      args="$(arg optical_translate) $(arg optical_rotate) base_link stereo_camera 100" />


<group ns="/stereo_camera" >
    <node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
        <remap from="left/image"        to="left/image_raw"/>
        <remap from="right/image"       to="right/image_raw"/>
        <remap from="left/camera_info"  to="left/camera_info"/>
        <remap from="right/camera_info" to="right/camera_info"/>

       
       
    </node>


    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
        <remap from="left/image_raw"    to="left/image_raw_throttle"/>
        <remap from="left/camera_info"  to="left/camera_info_throttle"/>
        <remap from="right/image_raw"   to="right/image_raw_throttle"/>
        <remap from="right/camera_info" to="right/camera_info_throttle"/>
    </node>


    <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/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"/>

       
       
       
    </node>
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet">
            <remap from="cloud" to="cloudXYZ"/>
        <remap from="obstacles" to="/planner_cloud"/>

       
       
       
       
       
    </node>


    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="left/image_rect"       to="left/image_rect"/>
        <remap from="right/image_rect"      to="right/image_rect"/>
        <remap from="left/camera_info"      to="left/camera_info"/>
        <remap from="right/camera_info"     to="right/camera_info"/>

       
       
       
       

       
       
       
       
    </node>
</group>

<group ns="rtabmap">
  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
     
     
     
     

     <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
     <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
     <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>

     <remap from="odom" to="/stereo_camera/odom"/>

     

     
     
     

     
     
  </node>

 
  <node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
     
     <remap from="mapData" to="mapData_optimized"/>
     <remap from="grid_projection_map" to="/map"/>
  </node>

</group>


<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/jtl_stereo.rviz"/>


</launch>

[matlabbe]

Hi,

1. Is base_footprint transform published on TF? What is your TF tree? Well, all rtabmap nodes have not "frame_id" redefined, which is "base_link" by default. If you have only base_link published, you may change the "base_footprint" frame in the yaml files to "base_link".

2. "stereo_nodelet" is not created (at least in the launch file provided), so move_base will not receive the "/planner_cloud" for the local costmap.

3. The map_assembler node is not connected in the graph (looks like map_optimizer missing). In current rtabmap_ros version, you don't need map_optimizer or map_assembled. You can connect directly the "rtabmap/proj_map" to "/map" used by move_base so the global costmap can be created. To have the point cloud, rtabmap node publishes "rtabmap/cloud_map" for convenience. See the doc for corresponding parameters.

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
     <remap from="proj_map" to="/map"/>
...

cheers

[Markus]

Thank you so much.
I highly appreciate your help and I cannot tell
how great your support is you offer.

Now, the system is running without errors or warnings.

The Graph looks as follows:


For the sake of sharing I provide here my "final" launch file:

<launch>


<arg name="rviz" default="true" />
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="-$(arg pi/2) 0 -$(arg pi/2)" />
<arg name="optical_translate" value="-0.10 0 0.72" />
<arg name="sim" default="false" />




<group ns="planner">
    <remap from="openni_points" to="/planner_cloud"/>
    <remap from="base_scan" to="/base_scan"/>
    <remap from="map" to="/map"/>
    <remap from="move_base_simple/goal" to="/planner_goal"/>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/local_costmap_params_3d.yaml" command="load" />
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/global_costmap_params.yaml" command="load" />
      <rosparam file="$(find rtabmap_ros)/launch/pioneer3/config/base_local_planner_params.yaml" command="load" />
    </node>

</group>


<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
      args="$(arg optical_translate) $(arg optical_rotate) base_link stereo_camera 100" />


<group ns="/stereo_camera" >
    <node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle stereo_nodelet">
        <remap from="left/image"        to="left/image_raw"/>
        <remap from="right/image"       to="right/image_raw"/>
        <remap from="left/camera_info"  to="left/camera_info"/>
        <remap from="right/camera_info" to="right/camera_info"/>

       
       
    </node>


    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
        <remap from="left/image_raw"    to="left/image_raw_throttle"/>
        <remap from="left/camera_info"  to="left/camera_info_throttle"/>
        <remap from="right/image_raw"   to="right/image_raw_throttle"/>
        <remap from="right/camera_info" to="right/camera_info_throttle"/>
    </node>


    <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>
    <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/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"/>

       
       
       
    </node>
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet">
            <remap from="cloud" to="cloudXYZ"/>
        <remap from="obstacles" to="/planner_cloud"/>

       
       
       
       
       
    </node>


    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="left/image_rect"       to="left/image_rect"/>
        <remap from="right/image_rect"      to="right/image_rect"/>
        <remap from="left/camera_info"      to="left/camera_info"/>
        <remap from="right/camera_info"     to="right/camera_info"/>

       
       
       
       

       
       
       
       
    </node>
</group>

<group ns="rtabmap">
  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
     
     
     
     

     <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
     <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
     <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>

     <remap from="odom" to="/stereo_camera/odom"/>
     <remap from="proj_map" to="/map"/>

     

     
     
     

     
     
  </node>


  <node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
     
     <remap from="mapData" to="mapData_optimized"/>
     <remap from="grid_projection_map" to="/map"/>
  </node>

</group>


<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/jtl_stereo.rviz"/>


</launch>

[matlabbe]

Great! :)