ZEDm Camera with Lidar setup for RTABMAP

[seanxu112]

Hi,

Currently, I am trying to use a ZEDm camera (with inertial odometry and visual odometry) as a stereo camera and a laser scan sensor. There are still some loop closure issues with the ZEDm itself but it is manageable for now. However, when I try to integrate with LIDAR, it just spits out this problem

[ WARN] (2019-07-09 17:31:48.165) OccupancyGrid.cpp:357::createLocalMap() Cannot create local map, scan is empty (node=13).

And 3D map and Occupancy Grid does not show up at all in rviz. In rtabmapviz the camera video stream does show up, but does not store the point cloud over time.

This is how I am utilizing stereo_mapping.launch:

       <include file="$(find rtabmap_ros)/launch/stereo_mapping.launch" >
                <arg name="stereo_namespace"  value="/zed/zed_node"/>
                <arg name="odom_topic"        value="/zed/zed_node/odom"/>
                <arg name="visual_odometry"   value="false"/>
                <arg name="odom_frame_id"     value="zed_pose_odom"/>
                <arg name="localization"      value="false"/>
                <arg name="rtabmap_args"      value="--delete_db_on_start"/>
                <arg name="scan_topic"      value="/scan"/>
                <arg name="subscribe_scan"    value="true"/>
        </include>

I also checked that scan topic is publishing the laser scan messages.  

Any feedback would be appreciated.

Sean Xu

[matlabbe]

Hi,

Are there valid range values in /scan (or they are all INF or 0)?

$ rostopic echo /scan

cheers,
Mathieu

[seanxu112]

This is a snippet of the output of rostopic echo /scan:

scan_time: 0.123400002718
range_min: 0.10000000149
range_max: 16.0
ranges: [0.0, 3.697000026702881, 3.742000102996826, 3.7850000858306885, 3.809000015258789, 3.8559999465942383, 3.9110000133514404, 0.0, 3.944999933242798, 3.996999979019165, 1.7480000257492065, 1.7300000190734863, 0.0, 1.7359999418258667, 1.7319999933242798, 1.7369999885559082, 1.75,..........

[matlabbe]

Hi,

I overlooked the original post. stereo_mapping.launch is deprecated, use rtabmap.launch directly. It offers a lot more flexbility, in particular if you want to combine a laser scan. Try this:

<include file="$(find rtabmap_ros)/launch/rtabmap.launch" >
    <arg name="stereo"            value="true"/>
    <arg name="stereo_namespace"  value="/zed/zed_node"/>
    <arg name="odom_topic"        value="/zed/zed_node/odom"/>
    <arg name="right_image_topic" value="/zed/zed_node/right/image_rect_color"/>
    <arg name="rgbd_sync"         value="true"/>
    <arg name="approx_rgbd_sync"  value="false"/>
    <arg name="approx_sync"       value="true"/>
    <arg name="subscribe_rgbd"    value="true"/>
    <arg name="visual_odometry"   value="false"/>
    <arg name="odom_frame_id"     value="zed_pose_odom"/>
    <arg name="localization"      value="false"/>
    <arg name="rtabmap_args"      value="--delete_db_on_start"/>
    <arg name="scan_topic"        value="/scan"/>
    <arg name="subscribe_scan"    value="true"/>
</include>

cheers,
Mathieu

[seanxu112]

Hi Mathieu,

Oh wow, thank you so much! Laser scan shows up now, so everything works properly. What exactly was the problem with stereo_mapping.launch?

cheers,
Sean

[matlabbe]

The subscribe_scan with stereo_mapping.launch is not working anymore. When synchronizing stereo and laser scan, we force that stereo topics should be pre-synchronized with rgbd_sync nodelet to make sure stereo images are exactly synchronized together before synchronizing with laser scan.

[seanxu112]

That makes sense. Thank you!

Sean

[ktucker]

can i just say that mattlabbe is a goddamn champion amongst men. thank you for your help, we luv rtabbemap

[seanxu112]

Hello,
With these parameters, we are getting an issue that says stereo and rgbd should not be subscribed to at the same time and it automatically sets subscribe_stereo to false. Is this related to another error we are getting where we are getting a rejected loop closure and nothing published to /rtabmap/localization_pose when localizing?
Thank you!

[matlabbe]

Hi,

No it is not related. It just means it sets subscribe_stereo to false for convenience. rtabmap.launch can do a lot different configurations for convenience, but some cases like that the check of compatibility of parameters is delegated to the nodes.

For the loop closure rejections, the warning is more an informative message than an error, to let us know that a loop closure is rejected (wrongly or not). If the loop closure should be good but rejected, we would need to understand what the message is telling us, then see why it happened.

cheers,
Mathieu

[seanxu112]

Hi Mathieu,

It seems like /rtabmap/localization_pose is not publishing consistently. Does it only publishes when there is loop closure? Also is that the node we should subscribe to for localization? Sometimes it does not get published for 10 seconds, and it is causing some problems in navigation stack, since the robot does not think it is moving. My last question is what kind of covariance should we expect from rtabmap localization? My autocovariance for one direction is around 0.004m when the robot is static.

Sincerely,
Sean

[matlabbe]

Hi,

The localization_pose is published only on localization/loop closure. The covariance means two things depending in which mode the node is:
 * Localization mode:  it is the variance of the transform computed
 * Mapping mode: it is the marginal covariance of the last node computed after graph optimization

If you want to pose in the map at any time, use TF /map -> /base_link which is always available. localization_pose is not meant to be published continuously, but discretely like a GPS (which can not be always available).

cheers,
Mathieu