Re: Navigation Stack + rtabmap requesting map configuration problem

Posted by mjedmonds on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Navigation-Stack-rtabmap-requesting-map-configuration-problem-tp1378p1471.html

Great, got that sorted out!

Now on to the next problem...is there a reason /rtabmap/cloud_map wouldn't begin publishing immediately?

I run the launch file below (updated to v0.11 as you recommended). I'm trying to use the navigation stack, with an observation source of /rtabmap/cloud_map (since I'm using a Kinect). After launching rtabmap, subscribing to the /rtabmap/cloud_map topic yields the following warning:

[ WARN] [1466095980.174555762]: Cloud map is empty! (poses=1 clouds=1)

After a certain amount of time, but cloud_map is eventually published. Do you have any guess as to why the cloud_map isn't immediately available, but becomes available later? I can provide a more detailed log of what's happening if necessary. I'm running in localization mode with the following launch file:

rtabmap.launch

<launch>

  <!-- additional params -->
  <arg name="publish_odom_tf" default="true"/>
  <arg name="publish_map_tf" default="true"/>

  <!-- Localization-only mode -->
  <arg name="localization"            default="true"/>

  <arg if="$(arg localization)" name="mapping" value="false" />
  <arg unless="$(arg localization)" name="mapping" value="true" />

  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For rgbd:=true
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
           
  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
     
  <!-- Choose between RGB-D and stereo -->      
  <arg name="rgbd"            default="true"/>
  <arg name="stereo"          default="false"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.5"/>

  <arg if="$(arg localization)" name="rtabmap_args" value="" />
  <arg unless="$(arg localization)" name="rtabmap_args" value="--delete_db_on_start" />

  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the stereo images are not synchronized -->
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
                                                                <!-- For depth_topic, "compressedDepth" image_transport is used. --> 
                                                                <!-- For rgb_topic, see "rgb_image_transport" argument. -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
   
  <arg name="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>

  <!-- algorithm params -->
  <arg name="strategy" default="0" />
  <arg name="feature" default="6" />
  <arg name="nn" default="3" />
  <arg name="max_depth" default="6.0" />
  <arg name="min_inliers" default="20" />
  <arg name="inlier_distance" default="0.02" />
  <arg name="local_map" default="1000" />
  <arg name="gftt_max_corners" default="1000" />
  <arg name="gftt_min_distance" default="7" />

  <!-- Nodes -->
  <group ns="$(arg namespace)">

  
    <!-- RGB-D Odometry -->
    <group if="$(arg rgbd)">
      <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
      <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
        <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
        <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
        <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>

        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
        <param name="publish_tf"                  type="bool"   value="$(arg publish_odom_tf)" />      

        <!-- Force 2D odometry -->
        <param name="Reg/Force2D" type="string" value="true"/> 
        <param name="Reg/Force3DoF" type="string" value="true"/> 

        <param name="Vis/EstimationType" type="string" value="$(arg strategy)"/>
        <param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
        <param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>  
        <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/> 
        <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
        <param name="OdomF2M/LocalHistorySize" type="string" value="$(arg local_map)"/>
        <param name="Vis/MaxFeatures" type="string" value="$(arg gftt_max_corners)"/>
        <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
      </node>
    </group>
    
    <!-- Stereo Odometry -->
    <group if="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
      <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
        <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
        <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
        <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
        <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
      </node>
    </group>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_map_tf)" />      
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="stereo_approx_sync"   type="bool"   value="$(arg approx_sync)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>

      <!-- Force 2D mapping  (constrained to x-y plane + yaw) -->
      <param name="Reg/Force3DoF" type="string" value="true"/>
      <param name="Reg/Force2D" type="string" value="true"/>
      <param name="Optimizer/Slam2D" type="string" value="true"/>

      <param name="Rtabmap/DetectionRate" type="string" value="3.0" />
      <param name="DbSqlite3/InMemory" type="string" value="true" />
      <param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.02"/>
    <param if="$(arg stereo)" name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>