Configure path planning

[ievolve]

When we call "set_goal" it appears to return an array of waypoints leading to the goal, and nav2 is directed to pass through those waypoints to the goal.  RTAB appears to be providing path planning.

I'd like to let nav2 do all the path planning to the goal.  It would be ideal if RTAB simply published and kept the end goal in sync.  Is this possible?

[matlabbe]

Hi,

You could make sure to keep rtabmap and nav2 disconnected (e.g., setting use_action_for_goal to false). When using set_goal or sending a pose on "goal" topic, rtabmap will plan in its graph a path to reach that goal. The goal cannot be farther than "RGBD/LocalRadius" (default 10 meters) from the map's graph, otherwise, send the goal directly to nav2.

So for why does rtabmap implement its own path planning? it is explained in this paper. In summary, if memory management is enabled and/or we do navigation while doing SLAM (not just localization on a static map) and/or RGBD/OptimizeFromGraphEnd=true, the pose we send to nav2 may change when a loop closure happens, causing the whole map to deform, so the pose should be updated to match the new map. When sending a goal pose through rtabmap, it will take care of that, by republishing the corrected pose of the goal when the map is updated.

As you said, rtabmap doesn't publish only the last pose of the plan, but publishes waypoints to reach the final pose. That is on purpose, it forces the robot to follow the mapping path, to increase the chance of re-localization. Those waypoints can be found in the published topic "global_path". To publish only the last pose to nav2, you could have an intermediate node subscribing to global_path, and republishing to nav2 the last pose when it detects it has changed.

By the way, just found there is maybe a bug with integration of nav2's action server with rtabmap, because as soon rtabmap sends a second goal (to overwrite the first one while following the path), it receives "ABORTED" state from the previous goal, and thus cancelling all the other poses on the path. Maybe related to that post, though maybe this has been fixed by now or I am not using nav2 action server right.

cheers,
Mathieu

[ievolve]

Thanks Mattieu!

So if I understand correctly, when "use_action_for_goal" is false, nav2 should NOT start navigating when the "set_goal' service is called?  

My experience is that rtab always sends global_path to nav2 and begins navigating irregardless if "use_action_for_goal" is true or false.  I think I just dont understand fully how this flag should behave.

Perhaps I should just monitor the /labels topic and pull off the negative id's (these are the labels) and send the pose of the label directly to nav2?  Would that work?

[teelusk]

Mathieu,

I'm having the same issue where rtabmap is triggering navigation, even if I have that `use_action_for_goal` set to false.

I did also test using the `send_goal` service, noting the last pose in that response and compared it to the corresponding value in `/labels` topic and it doesn't appear that these poses match (see example below, formatted for readability)

## From /labels
{
    "pose": {
        "position": {
            "x": -1.5404587984085083,
            "y": -0.8287004828453064,
            "z": 0.0
        },
        "orientation": {
            "x": 0.0,
            "y": 0.0,
            "z": 0.0,
            "w": 1.0
        }
    }
}


## Final pose from set_goal response
{
    "pose": {
        "position": {
            "x": -1.5439865589141846,
            "y": -0.9379720687866211,
            "z": 0.0
        },
        "orientation": {
            "x": 0.0,
            "y": 0.0,
            "z": -0.4504116365085988,
            "w": 0.8928210305676616
        }
    }
}

[matlabbe]

ievolve wrote

So if I understand correctly, when "use_action_for_goal" is false, nav2 should NOT start navigating when the "set_goal' service is called?  

It could if you remapped rtabmap's  "goal_out" topic to input "goal_pose" topic of nav2. The global_path is not connected to nav2, so if it is on your setup, it is by coincidence. With this turtlebot4 demo, it is connected to nav2:

ros2 topic info --verbose /global_path
Type: nav_msgs/msg/Path

Publisher count: 1

Node name: rtabmap
Node namespace: /
Topic type: nav_msgs/msg/Path
Endpoint type: PUBLISHER
GID: 01.0f.70.28.87.4d.88.b6.00.00.00.00.00.00.37.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 1

Node name: rtabmap_viz
Node namespace: /
Topic type: nav_msgs/msg/Path
Endpoint type: SUBSCRIPTION
GID: 01.0f.70.28.89.4d.2f.a6.00.00.00.00.00.00.37.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

When use_action_for_goal is true, it means rtabmap will connect to nav2 action server to publish the goals. If use_action_for_goal is false, then only the topic "goal_out" would be published by rtabmap node when requesting a plan. You can doublecheck if that parameter is correctly enabled/disabled by calling:

ros2 param get /rtabmap use_action_for_goal
Boolean value is: False

Note sure /labels would fix your issue. That topic purpose is mostly to visualize labels and node ids on Rviz as markers (when combining with MapGraph display). When playing with labels, it would more convenient to use list_labels and set_label services. Then use set_goal service by setting a label instead of a node_id.

$ ros2 service call /rtabmap/set_goal rtabmap_msgs/srv/SetGoal "{node_label: map0}"
waiting for service to become available...
requester: making request: rtabmap_msgs.srv.SetGoal_Request(node_id=0, node_label='map0', frame_id='')

response:
rtabmap_msgs.srv.SetGoal_Response(path_ids=[28, 18, 1], path_poses=[
geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-1.1560359001159668, y=2.9091389179229736, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.8471848224432924, w=0.5312983546970538)), 
geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.6467797756195068, y=0.3061254024505615, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.7951843455749338, w=0.606367957054416)), 
geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=0.01847362518310547, y=0.001550436019897461, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=-0.00019708280454233746, w=0.9999999850988387))], planning_time=0.000640869140625)

I still think having a node subscribing to global_path, and just republishing the last goal in that path to nav2 would do what you want.

teelusk wrote

I did also test using the `send_goal` service, noting the last pose in that response and compared it to the corresponding value in `/labels` topic and it doesn't appear that these poses match (see example below, formatted for readability)

I tested it here:

$ ros2 service call /rtabmap/set_goal rtabmap_msgs/srv/SetGoal "{node_id: 18}"
waiting for service to become available...
requester: making request: rtabmap_msgs.srv.SetGoal_Request(node_id=18, node_label='', frame_id='')

response:
rtabmap_msgs.srv.SetGoal_Response(path_ids=[28, 18], path_poses=[
geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-1.1560359001159668, y=2.9091389179229736, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.8471848224432924, w=0.5312983546970538)), 
geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.6467797756195068, y=0.3061254024505615, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.7951843455749338, w=0.606367957054416))], planning_time=0.0007572174072265625)

ros2 topic echo /labels
markers:
[...]
- header:
    stamp:
      sec: 1221
      nanosec: 954000000
    frame_id: map
  ns: ids
  id: 18
  type: 9
  action: 0
  pose:
    position:
      x: -0.6494603157043457
      y: 0.3056384325027466
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  scale:
    x: 1.0
    y: 1.0
    z: 0.2
  color:
    r: 1.0
    g: 1.0
    b: 1.0
    a: 0.5
  lifetime:
    sec: 2
    nanosec: 0
  frame_locked: false
  points: []
  colors: []
  texture_resource: ''
  texture:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    format: ''
    data: []
  uv_coordinates: []
  text: '18'
  mesh_resource: ''
  mesh_file:
    filename: ''
    data: []
  mesh_use_embedded_materials: false
[...]
- header:
    stamp:
      sec: 1221
      nanosec: 954000000
    frame_id: map
  ns: ids
  id: 28
  type: 9
  action: 0
  pose:
    position:
      x: -1.1560359001159668
      y: 2.9091389179229736
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  scale:
    x: 1.0
    y: 1.0
    z: 0.2
  color:
    r: 1.0
    g: 1.0
    b: 1.0
    a: 0.5
  lifetime:
    sec: 2
    nanosec: 0
  frame_locked: false
  points: []
  colors: []
  texture_resource: ''
  texture:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ''
    format: ''
    data: []
  uv_coordinates: []
  text: '28'
  mesh_resource: ''
  mesh_file:
    filename: ''
    data: []
  mesh_use_embedded_materials: false

The poses look pretty close. Showing MapGraph and labels at same time: