Re: Configure path planning

Posted by matlabbe on Sep 12, 2024; 2:57am
URL: http://official-rtab-map-forum.206.s1.nabble.com/Configure-path-planning-tp10430p10439.html

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: