Kinect + Odometry + 2D laser help please!

[bj-neilson]

Hi,
I've been trying to implement this example using a ZED camera, Odometry from MAVROS (remap from mavros, data coming from Cube/Ardurover) and an RPLidarA2.

I tried to create a single launch file (zed_rtabmap_mower.launch) to launch the lidar, camera then RTABMap.  Kept having trouble so took out the lidar.

Now running Roscore in a tab, then rplidar.launch, then zed_rtabmap_mower.launch.

zed_rtabmap_mower.launch calls rtabmap_mower.launch, which is essentially the example provided on the tutorial page.
--------------------------------------------------------------------------------
rplidar.launch

<launch>
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
 
 
 
 
 
 
  </node>
</launch>
-------------------------------------------------------------------------------------------------------------

zed_rtabmap_mower.launch

<?xml version="1.0"?>

<launch>
    <arg name="zed_namespace"     default="zed" />   

    <arg name="svo_file"                        default="" /> 

    <arg name="zed_node_name"                   default="zed_node" />
    <arg name="camera_model"                    default="zed" /> 
    <arg name="publish_urdf"                    default="true" />

    <group ns="$(arg zed_namespace)">
       
        <include file="$(find zed_wrapper)/launch/zed_camera.launch">
            <arg name="svo_file"            value="$(arg svo_file)" />
            <arg name="node_name"           value="$(arg zed_node_name)" />
            <arg name="camera_model"        value="$(arg camera_model)" />
            <arg name="publish_urdf"        value="$(arg publish_urdf)" />
        </include>
    </group>

   
 
 
       
    <include file="$(find rtabmap_ros)/launch/rtabmap_mower.launch">
    </include>

</launch>
----------------------------------------------------------------------------------------------------

rtabmap_mower.launch

<launch>
  <group ns="rtabmap">

   
   
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="/zed/zed_node/rgb/image_rect_color"/>
      <remap from="depth/image"     to="/zed/zed_node/depth/depth_registered"/>
      <remap from="rgb/camera_info" to="/zed/zed_node/rgb/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/> 
     
     
       
    </node>

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

         
         
         

          <remap from="odom" to="/base_controller/odom"/>
          <remap from="scan" to="/base_scan"/>
          <remap from="rgbd_image" to="rgbd_image"/>

         

         
         
         
         
         
         
           
         
           
         
         
         
         
    </node>
  </group>
</launch>
------------------------------------------------------------------------------------------------------------------------
Running these I get no map generated.  

The first warning I see only once...

[ WARN] [1562103125.323454264]: /rtabmap/rgbd_sync: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rgbd_sync subscribed to (exact sync):
   /zed/zed_node/rgb/image_rect_color,
   /zed/zed_node/depth/depth_registered,
   /zed/zed_node/rgb/camera_info

Then this warning every 5 sec.....

[ WARN] [1562103254.245377472]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=200).
/rtabmap/rtabmap subscribed to (approx sync):
   /base_controller/odom,
   /rtabmap/rgbd_image,
   /base_scan



------------------------------------------------------------------------------------------------------
The 3 zed topics mentioned first are publishing data.

/base_controller/odom is not publishing.  I removed the remap line from /mavros/imu as I was getting confused as to where to put it.
/rtabmap/rgbd_image is publishing
/base_scan is not publishing, however /scan is publishing.

I don't seem to have anything subscribed to the lidar.  Or is it a TF issue?  



Can anyone please offer any guidance to fix this? I'd like to map using the camera.  Use the IMU and correct that via ICP using lidar for odometry.  Then use the lidar also for occupancy grid.  Thank you in advance for any help you could offer!

[matlabbe]

Hi,

You may have to change those remap:

<remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/>

At least for scan, it could be "/scan" instead of "/base_scan". For odom, look at what odometry is called from zed node.

cheers,
Mathieu

[bj-neilson]

Thanks Mathieu, I’ll try that.

Ta,
Ben

[bj-neilson]

Got it going thank you Mathieu

[datadrian]

I'm having a similar issue while trying to use my linorobot base and RTAB. I'm getting the same messages about odom, scan and rgbd. [ WARN] [1562705732.724543359]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10). /rtabmap/rtabmap subscribed to (approx sync): /base_controller/odom, /rtabmap/rgbd_image, /base_scan When I look at my topics when i do bringup all those topics exist

/base_controller/odom
/base_scan
/camera/depth/camera_info
/camera/depth/disparity
/camera/depth/image
/camera/depth/image/compressed
/camera/depth/image/compressed/parameter_descriptions
/camera/depth/image/compressed/parameter_updates
/camera/depth/image/compressedDepth
/camera/depth/image/compressedDepth/parameter_descriptions
/camera/depth/image/compressedDepth/parameter_updates
/camera/depth/image_raw
/camera/depth/image_raw/compressed
/camera/depth/image_raw/compressed/parameter_descriptions
/camera/depth/image_raw/compressed/parameter_updates
/camera/depth/image_raw/compressedDepth
/camera/depth/image_raw/compressedDepth/parameter_descriptions
/camera/depth/image_raw/compressedDepth/parameter_updates
/camera/depth/image_rect
/camera/depth/image_rect/compressed
/camera/depth/image_rect/compressed/parameter_descriptions
/camera/depth/image_rect/compressed/parameter_updates
/camera/depth/image_rect/compressedDepth
/camera/depth/image_rect/compressedDepth/parameter_descriptions
/camera/depth/image_rect/compressedDepth/parameter_updates
/camera/depth/image_rect_raw
/camera/depth/image_rect_raw/compressed
/camera/depth/image_rect_raw/compressed/parameter_descriptions
/camera/depth/image_rect_raw/compressed/parameter_updates
/camera/depth/image_rect_raw/compressedDepth
/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/camera/depth/points
/camera/depth_rectify_depth/parameter_descriptions
/camera/depth_rectify_depth/parameter_updates
/camera/depth_registered/camera_info
/camera/depth_registered/disparity
/camera/depth_registered/hw_registered/image_rect
/camera/depth_registered/hw_registered/image_rect/compressed
/camera/depth_registered/hw_registered/image_rect/compressed/parameter_descriptions
/camera/depth_registered/hw_registered/image_rect/compressed/parameter_updates
/camera/depth_registered/hw_registered/image_rect/compressedDepth
/camera/depth_registered/hw_registered/image_rect/compressedDepth/parameter_descriptions
/camera/depth_registered/hw_registered/image_rect/compressedDepth/parameter_updates
/camera/depth_registered/hw_registered/image_rect_raw
/camera/depth_registered/hw_registered/image_rect_raw/compressed
/camera/depth_registered/hw_registered/image_rect_raw/compressed/parameter_descriptions
/camera/depth_registered/hw_registered/image_rect_raw/compressed/parameter_updates
/camera/depth_registered/hw_registered/image_rect_raw/compressedDepth
/camera/depth_registered/hw_registered/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth_registered/hw_registered/image_rect_raw/compressedDepth/parameter_updates
/camera/depth_registered/image
/camera/depth_registered/image/compressed
/camera/depth_registered/image/compressed/parameter_descriptions
/camera/depth_registered/image/compressed/parameter_updates
/camera/depth_registered/image/compressedDepth
/camera/depth_registered/image/compressedDepth/parameter_descriptions
/camera/depth_registered/image/compressedDepth/parameter_updates
/camera/depth_registered/image_raw
/camera/depth_registered/image_raw/compressed
/camera/depth_registered/image_raw/compressed/parameter_descriptions
/camera/depth_registered/image_raw/compressed/parameter_updates
/camera/depth_registered/image_raw/compressedDepth
/camera/depth_registered/image_raw/compressedDepth/parameter_descriptions
/camera/depth_registered/image_raw/compressedDepth/parameter_updates
/camera/depth_registered/points
/camera/depth_registered/sw_registered/camera_info
/camera/depth_registered/sw_registered/image_rect
/camera/depth_registered/sw_registered/image_rect/compressed
/camera/depth_registered/sw_registered/image_rect/compressed/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect/compressed/parameter_updates
/camera/depth_registered/sw_registered/image_rect/compressedDepth
/camera/depth_registered/sw_registered/image_rect/compressedDepth/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect/compressedDepth/parameter_updates
/camera/depth_registered/sw_registered/image_rect_raw
/camera/depth_registered/sw_registered/image_rect_raw/compressed
/camera/depth_registered/sw_registered/image_rect_raw/compressed/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect_raw/compressed/parameter_updates
/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth
/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth_registered/sw_registered/image_rect_raw/compressedDepth/parameter_updates
/camera/depth_registered_rectify_depth/parameter_descriptions
/camera/depth_registered_rectify_depth/parameter_updates
/camera/driver/parameter_descriptions
/camera/driver/parameter_updates
/camera/ir/camera_info
/camera/ir/image_raw
/camera/ir/image_raw/compressed
/camera/ir/image_raw/compressed/parameter_descriptions
/camera/ir/image_raw/compressed/parameter_updates
/camera/ir/image_raw/compressedDepth
/camera/ir/image_raw/compressedDepth/parameter_descriptions
/camera/ir/image_raw/compressedDepth/parameter_updates
/camera/ir/image_rect_ir
/camera/ir/image_rect_ir/compressed
/camera/ir/image_rect_ir/compressed/parameter_descriptions
/camera/ir/image_rect_ir/compressed/parameter_updates
/camera/ir/image_rect_ir/compressedDepth
/camera/ir/image_rect_ir/compressedDepth/parameter_descriptions
/camera/ir/image_rect_ir/compressedDepth/parameter_updates
/camera/ir_rectify_ir/parameter_descriptions
/camera/ir_rectify_ir/parameter_updates
/camera/projector/camera_info
/camera/rgb/camera_info
/camera/rgb/image_color
/camera/rgb/image_color/compressed
/camera/rgb/image_color/compressed/parameter_descriptions
/camera/rgb/image_color/compressed/parameter_updates
/camera/rgb/image_color/compressedDepth
/camera/rgb/image_color/compressedDepth/parameter_descriptions
/camera/rgb/image_color/compressedDepth/parameter_updates
/camera/rgb/image_mono
/camera/rgb/image_mono/compressed
/camera/rgb/image_mono/compressed/parameter_descriptions
/camera/rgb/image_mono/compressed/parameter_updates
/camera/rgb/image_mono/compressedDepth
/camera/rgb/image_mono/compressedDepth/parameter_descriptions
/camera/rgb/image_mono/compressedDepth/parameter_updates
/camera/rgb/image_raw
/camera/rgb/image_raw/compressed
/camera/rgb/image_raw/compressed/parameter_descriptions
/camera/rgb/image_raw/compressed/parameter_updates
/camera/rgb/image_raw/compressedDepth
/camera/rgb/image_raw/compressedDepth/parameter_descriptions
/camera/rgb/image_raw/compressedDepth/parameter_updates
/camera/rgb/image_rect_color
/camera/rgb/image_rect_color/compressed
/camera/rgb/image_rect_color/compressed/parameter_descriptions
/camera/rgb/image_rect_color/compressed/parameter_updates
/camera/rgb/image_rect_color/compressedDepth
/camera/rgb/image_rect_color/compressedDepth/parameter_descriptions
/camera/rgb/image_rect_color/compressedDepth/parameter_updates
/camera/rgb/image_rect_mono
/camera/rgb/image_rect_mono/compressed
/camera/rgb/image_rect_mono/compressed/parameter_descriptions
/camera/rgb/image_rect_mono/compressed/parameter_updates
/camera/rgb/image_rect_mono/compressedDepth
/camera/rgb/image_rect_mono/compressedDepth/parameter_descriptions
/camera/rgb/image_rect_mono/compressedDepth/parameter_updates
/camera/rgb_debayer/parameter_descriptions
/camera/rgb_debayer/parameter_updates
/camera/rgb_rectify_color/parameter_descriptions
/camera/rgb_rectify_color/parameter_updates
/camera/rgb_rectify_mono/parameter_descriptions
/camera/rgb_rectify_mono/parameter_updates
/cmd_vel
/diagnostics
/imu/data
/imu/data_raw
/imu/mag
/imu_filter_madgwick/parameter_descriptions
/imu_filter_madgwick/parameter_updates
/odom
/pid
/raw_imu
/raw_odom
/raw_vel
/rosout
/rosout_agg
/rpms
/rtabmap/cloud_ground
/rtabmap/cloud_map
/rtabmap/cloud_obstacles
/rtabmap/global_path
/rtabmap/global_pose
/rtabmap/goal
/rtabmap/goal_node
/rtabmap/goal_out
/rtabmap/goal_reached
/rtabmap/grid_map
/rtabmap/grid_prob_map
/rtabmap/info
/rtabmap/initialpose
/rtabmap/labels
/rtabmap/local_path
/rtabmap/localization_pose
/rtabmap/mapData
/rtabmap/mapGraph
/rtabmap/mapPath
/rtabmap/move_base/cancel
/rtabmap/move_base/feedback
/rtabmap/move_base/goal
/rtabmap/move_base/result
/rtabmap/move_base/status
/rtabmap/octomap_binary
/rtabmap/octomap_empty_space
/rtabmap/octomap_full
/rtabmap/octomap_grid
/rtabmap/octomap_ground
/rtabmap/octomap_obstacles
/rtabmap/octomap_occupied_space
/rtabmap/proj_map
/rtabmap/rgbd_image
/rtabmap/rgbd_image/compressed
/rtabmap/scan_map
/rtabmap/user_data_async
/scan
/set_pose
/tf
/tf_static

Here's my launch file:

<launch>
    <!-- Start ROS communication between the robot's computer and Linorobot base -->
    <node pkg="rosserial_python" name="rosserial_lino" type="serial_node.py" output="screen">
        <param name="port" value="/dev/linobase" />
        <param name="baud" value="57600" />
    </node>

    <!-- IMU Relay and Filter -->
    <include file="$(find linorobot)/launch/include/imu/imu.launch" />

    <!-- Publish Linorobot odometry -->
    <node pkg="linorobot" name="lino_base_node" type="lino_base_node"></node>

    <!-- Publish static transform from base_footprint to base_link -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0.127 0 0 0  /base_footprint /base_link"/>

    <!-- Odom-IMU Extended Kalman Filter-->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
        <remap from="odometry/filtered" to="odom" />
        <rosparam command="load" file="$(find linorobot)/param/ekf/robot_localization.yaml" />
    </node>

    <!-- Run XV11 -->
    <include file="$(find linorobot)/launch/include/laser.launch" />

  <group ns="rtabmap">

    <!-- Use RGBD synchronization -->
    <!-- Here is a general example using a standalone nodelet,
         but it is recommended to attach this nodelet to nodelet
         manager of the camera to avoid topic serialization -->
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->

      <!-- Should be true for not synchronized camera topics
           (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-->
      <param name="approx_sync"       value="true"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_link"/>

          <param name="subscribe_depth" type="bool" value="false"/>
          <param name="subscribe_rgbd" type="bool" value="true"/>
          <param name="subscribe_scan" type="bool" value="true"/>

          <remap from="odom" to="/base_controller/odom"/>
          <remap from="scan" to="/base_scan"/>
          <remap from="rgbd_image" to="rgbd_image"/>

          <param name="queue_size" type="int" value="10"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
          <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
          <param name="Grid/FromDepth"            type="string" value="false"/> <!-- occupancy grid from lidar -->
          <param name="Reg/Force3DoF"             type="string" value="true"/>
          <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->

          <!-- ICP parameters -->
          <param name="Icp/VoxelSize"                 type="string" value="0.05"/>
          <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
    </node>
  </group>
</launch>

Am I just missing a slash in those remaps? Obviously this is one of my first stabs into ros, please forgive me. bte, I'm just getting Rtab working with kinect before i move onto a realsense for outdoor mapping.

[matlabbe]

Hi,

Based on your setup, it could more like this:

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_link"/>

          <param name="subscribe_depth" type="bool" value="false"/>
          <param name="subscribe_rgbd" type="bool" value="true"/>
          <param name="subscribe_scan" type="bool" value="true"/>

          <remap from="odom" to="/odometry/filtered"/>
          <remap from="scan" to="/scan"/>
          <remap from="rgbd_image" to="rgbd_image"/>

          <param name="queue_size" type="int" value="10"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
          <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
          <param name="Grid/FromDepth"            type="string" value="false"/> <!-- occupancy grid from lidar -->
          <param name="Reg/Force3DoF"             type="string" value="true"/>
          <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->

          <!-- ICP parameters -->
          <param name="Icp/VoxelSize"                 type="string" value="0.05"/>
          <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
    </node>

cheers,
Mathieu

[datadrian]

unfortunately it just says its not getting messages for those remaps too.

 

matlabbe wrote

For rgbd_sync, I would set the following as you seem using kinect for xbox 360:

<param name="approx_sync"       value="false"/>

so you're say this should be set to true, per I'm using xbox 360 kinect?

setting it false produces more unlinked errors, so the correct setting of true has less errors.

yet still my odom,scan and rgbd remain unlinked despite info being publish and viewable.

any other ideas? something i could show you?

thanks for your help thus far.

-adrian

[matlabbe]

Hi,

I removed the comment on rgbd_sync of my last post, it should be indeed true for kinect v1.

Which topics are not published? Can you show the warnings? Can you show the "rostopic hz" for each topic?

[datadrian]

Here is the error and here is rostopic echo for /scan, /odom and /rtab/rgbd

it just seems the remaps aren't going through

[matlabbe]

Hi,

It will be easier to see if you do:

$ rostopic hz /base_controller/odom /rtabmap/rgbd_image /base_scan

Did you set the following to rtabmap node like in my previous post?

<remap from="odom" to="/odometry/filtered"/>
<remap from="scan" to="/scan"/>

[datadrian]

switching to those other values triggers the same error message but with the new remap names.

attached are images of ros topic for the remaps and the actual topics.

my scan frequency is now 50, also tried 60 but no still no dice.

thanks again for your support

[matlabbe]

Is rtabmap subscribed to /odom, /scan and /rtabmap/rgbd_image? If so, it should be able to run.

[datadrian]

forgive my ignorance, I thought doing these remaps was how i got rtab maps
to subscribe to my topics. if not here, how would i go about having rtab
map subscribe?

[matlabbe]

What is your warning message now with the remaps? What is your rqt_graph?

[datadrian]

same error i've had the whole time

[matlabbe]

/base_controller/odom and /base_scan are not hard-coded default topic names, you should have those remaps set somewhere in your launch files.

[datadrian]

This is my launch file, are these not the remaps? I'm not understanding why these don't work.

 <remap from="odom" to="/odometry/filtered"/>
          <remap from="scan" to="/scan"/>
          <remap from="rgbd_image" to="rgbd_image"/>

matlabbe wrote

/base_controller/odom and /base_scan are not hard-coded default topic names, you should have those remaps set somewhere in your launch files.

[matlabbe]

What are all the launch files you launch? It is difficult to help with only a small part...