RTABMap does not subscribe topics well after stopping and restarting rosmon

[iportolesrb]

Hi,

I'm using RTAB-Map with rosmon and a ZED2 camera, and it is subscribed to some topics (images and odometry). The RTAB-Map is running in a rosmon node that I start and stop when I want to create a new map using the rosmon start and stop services. When I start the node for the first time it goes well, but the problem is that if I stop it and in any moment start it again, the RTAB-Map does not map and this warning is prompted:

[CommonDataSubscriber::warningLoop]: /robot/zed2/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).
/robot/zed2/rtabmap subscribed to (approx sync):
             /robot/odom \
             /robot/zed2/zed_node/rgb/image_rect_color \
             /robot/zed2/zed_node/depth/depth_registered \
             /robot/zed2/zed_node/rgb/camera_info

The date and time in the are synchronized so that should not be the problem, and it happens always the second time but not the first. Is this happening with any other package and rosmon? Is there something in rosmon that could prevent a node to receive messages the second time?

Thank you in advance, Inés

[xqms]

Hi Inés, thanks for the report. I'm not aware of any rosmon feature that might cause this problem. The only thing I can think of is that there are some parameters in the launch file that are on the ROS parameter server and that are overwritten by the node itself. When you restart the node, it doesn't see the "clean" values from the launch file but its own values.

Can you share the launch file you are using?

[iportolesrb]

Hi @xqms thanks for the reply,

These are my launchfiles and .yaml:

zed_rtabmap.launch:

<launch>
    <arg name="zed_node_name"        default="zed_node" />

    <arg name="camera_name"          default="zed2" />

    <group ns="robot">
        <group ns="$(arg camera_name)">

            <!-- RTAB-Map -->
            <arg name="custom_rtabmap_launch_file" default="$(find project_bringup)/launch/perception/sl_rtabmap.launch.xml"/>
            <include file="$(arg custom_rtabmap_launch_file)">
                <arg name="odom_topic"              value="/robot/odom" />
                <arg name="rgb_topic"               value="$(arg zed_node_name)/rgb/image_rect_color" />
                <arg name="depth_topic"             value="$(arg zed_node_name)/depth/depth_registered" />
                <arg name="camera_info_topic"       value="$(arg zed_node_name)/rgb/camera_info" />
                <arg name="depth_camera_info_topic" value="$(arg zed_node_name)/depth/camera_info" />
            </include>

            <!-- Rviz -->
            <!--node name="rviz" pkg="rviz" type="rviz" args="-d $(find zed_rtabmap_example)/rviz/zed-rtabmap.rviz" output="screen" /--> 
        </group>
    </group>
</launch>

sl_rtabmap.launch.xml:

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

    <arg     if="$(arg localization)" name="args" default=""/>
    <arg unless="$(arg localization)" name="args" default="--delete_db_on_start"/>

    <!-- RGB-D related topics -->
    <arg name="rgb_topic"               default="/zed/zed_node/rgb/image_rect_color" />
    <arg name="depth_topic"             default="/zed/zed_node/depth/depth_registered" />
    <arg name="camera_info_topic"       default="/zed/zed_node/rgb/camera_info" />
    <arg name="depth_camera_info_topic" default="/zed/zed_node/depth/camera_info" />

    <arg name="odom_topic"              default="/zed/zed_node/odom" />    <!-- Odometry topic name -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)" launch-prefix="">
        <rosparam command="load" file="$(find project_bringup)/config/perception/rtabmap.yaml" />

        <remap from="rgb/image"         to="$(arg rgb_topic)"/>
        <remap from="depth/image"       to="$(arg depth_topic)"/>
        <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>

        <remap from="grid_map"          to="map" />
        <remap from="odom"              to="$(arg odom_topic)"/>

        <remap from="scan_cloud" to="/robot/top_3d_laser/rslidar_points"/>

        <!-- 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>
</launch>

rtabmap.yaml:

# RTAB-Map configuration

subscribe_depth:        true
subscribe_rgbd:         false
subscribe_stereo:       false
subscribe_stereo:       false
subscribe_scan:         false
subscribe_scan_cloud:   true
subscribe_user_data:    false
subscribe_odom_info:    false

database_path:      "~/.ros/rtabmap.db"
config_path:        "~/.ros/rtabmap.cfg"

frame_id:       "robot_base_link"
map_frame_id:   "world"
odom_frame_id:  "robot_odom"                   # odometry from odom msg to have covariance - Remapped by launch file
odom_tf_angular_variance:  0.001     # If TF is used to get odometry, this is the default angular variance
odom_tf_linear_variance:   0.001     # If TF is used to get odometry, this is the default linear variance
tf_delay:       0.02
publish_tf:     false                # Set to false if fusing different poses (map->odom) with UKF

odom_sensor_sync:               true
wait_for_transform_duration:    0.2
approx_sync:                    true

queue_size: 100

scan_normal_k:  0

Grid:
    3D:                     true
    FlatObstacleDetected:   true
    MapFrameProjection:     false
    GroundIsObstacle:       false
    PreVoxelFiltering:      true
    RayTracing:             true
    FromDepth:              true
    NormalsSegmentation:    false
    CellSize:               0.05
    ClusterRadius:          0.1
    MinClusterSize:         3
    DepthDecimation:        1
    DepthRoiRatios:         [0.0, 0.0, 0.0, 0.0]
    FootprintHeight:        2.0
    FootprintLength:        0.18
    FootprintWidth:         0.18
    MaxGroundAngle:         30.0
    MinGroundHeight:        -0.5
    MaxGroundHeight:        -0.4
    MaxObstacleHeight:      0.1
    NoiseFilteringMinNeighbors: 2
    NoiseFilteringRadius:   0.05
    NormalK:                20
    RangeMin:               0.7
    RangeMax:               10.0

GridGlobal:
    Eroded:                 false       # Erode obstacle cells
    FootprintRadius:        0.18        # Footprint radius (m) used to clear all obstacles under the graph
    FullUpdate:             true        # When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not
    MaxNodes:               0           # Maximum nodes assembled in the map starting from the last node (0=unlimited)
    MinSize:                1.0        # Minimum map size (m)
    OccupancyThr:           0.55        # Occupancy threshold (value between 0 and 1)
    ProbClampingMax:        0.971       # Probability clamping maximum (value between 0 and 1)
    ProbClampingMin:        0.1192      # Probability clamping minimum (value between 0 and 1)
    ProbHit:                0.7         # Probability of a hit (value between 0.5 and 1)
    ProbMiss:               0.4         # Probability of a miss (value between 0 and 0.5)
    UpdateError:            0.01        # Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value

[xqms]

And what exactly are you doing to restart the process? Are you restarting a single node using rosmon's UI? What happens when you restart the launch file containing the node?

[iportolesrb]

Hi @xqms I use the rosmon start_stop service to start or stop my node (I have only one node running in that rosmon call). I have tried to stop and start using the UI with "k" and "s" and the result is the same. But when I use Ctrl+C to kill the rosmon that launches the file and I call it again it works fine. The problem is that I need to start and stop the node automatically when an event occurs so I need to use the start_stop service.

[xqms]

Could you please do the following to debug this further:

1. Start the launch file
2. Stop it with CTRL+C
3. Execute rosparam dump param_dump1.yaml
4. Start the launch file again, but with mon launch --no-start LAUNCH_FILE
5. Stop it with CTRL+C
6. Execute rosparam dump param_dump2.yaml

and upload the two YAML files here. Hopefully we will see a difference ;)

[iportolesrb]

Hi @xqms

Sorry about the delay and thank you for your answer. Using that commands I can see that there are a few parameters that are deleted between one and other start, so it's likely that rtabmap delete its parameters when it stops. So I think it is a rtabmap issue then, thank you for your help.