Facing location compute problem while launch fleet adapter

[Anisha-thakkar]

System info:
Ubuntu'22
ROS2 Humble
rmf jazzy (src build)
I have successfully installed the rmf core setup, successfully ran all the demos of rmf_demos and free_fleet example too.
Now I am trying integrate the real robot with rmf. I have completed the zenoh bridge setup on the robot and communication is established. I have created the building.yaml map of the surrounding with the help of traffic editor and done the necessary change in fleet adapter but when I am trying to launch fleet adapter it throws initial state location error as following:

ros2 launch free_fleet_examples nav2_tb3_simulation_fleet_adapter.launch.xml  server_uri:="ws://localhost:8000/_internal"
[INFO] [launch]: All log files can be found below /home/einfochips/.ros/log/2025-02-04-16-12-20-108867-AHMCPU2642-234346
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [fleet_adapter.py-1]: process started with pid [234347]
[fleet_adapter.py-1] [retreat_to_charger_interval] value is not provided, default to 10 seconds
[fleet_adapter.py-1] Finishing request: charge
[fleet_adapter.py-1] Fleet is configured to perform ChargeBattery as finishing request
[fleet_adapter.py-1] Maximum delay is not provided, default to 10.0
[fleet_adapter.py-1] [INFO] [1738665740.957307021] [smartrover_fleet_adapter]: Parameter [discovery_timeout] set to: 60.000000
[fleet_adapter.py-1] [INFO] [1738665740.962790386] [smartrover_fleet_adapter]: Registering to query topic rmf_traffic/query_update_1
[fleet_adapter.py-1] [INFO] [1738665740.973868705] [smartrover_fleet_adapter]: Mirror handling new sync of 1 queries from schedule node [c2b944f0-73c9-4a10-840c-85985624f905]
[fleet_adapter.py-1] [INFO] [1738665741.995917870] [smartrover_command_handle]: Transformation error estimate for L1: 6.6409171819835375
[fleet_adapter.py-1] [INFO] [1738665742.000543613] [smartrover_fleet_adapter]: Attempting to connect to ws://localhost:8000/_internal
[fleet_adapter.py-1] [INFO] [1738665742.001114835] [smartrover_fleet_adapter]: Fleet [smartrover] is configured to perform delivery tasks
[fleet_adapter.py-1] [INFO] [1738665742.001148268] [smartrover_fleet_adapter]: Fleet [smartrover] is configured to perform patrol tasks
[fleet_adapter.py-1] [INFO] [1738665742.001195442] [smartrover_fleet_adapter]: Finished configuring Easy Full Control adapter for fleet [smartrover]
[fleet_adapter.py-1] [INFO] [1738665742.002766787] [smartrover_fleet_adapter]: Succesfully connected to ws://localhost:8000/_internal
[fleet_adapter.py-1] [INFO] [1738665742.002810608] [smartrover_fleet_adapter]: Connected to server
[fleet_adapter.py-1] [INFO] [1738665742.002837256] [smartrover_fleet_adapter]: Sent all updates
[fleet_adapter.py-1] [INFO] [1738665742.002857570] [smartrover_fleet_adapter]: Attempting queue flush if connected
[fleet_adapter.py-1] [INFO] [1738665742.007143429] [smartrover_command_handle]: Unable to get transform between base_footprint and map: <class 'tf2.LookupException'>: "sr1/map" passed to lookupTransform argument target_frame does not exist. 
[fleet_adapter.py-1] [INFO] [1738665742.007541245] [smartrover_command_handle]: Failed to update robot [sr1]: Unable to get transform between base_footprint and map
[fleet_adapter.py-1] [INFO] [1738665742.007895717] [smartrover_command_handle]: Failed to pose of robot [sr1]
[fleet_adapter.py-1] [INFO] [1738665742.107609545] [smartrover_command_handle]: Unable to get transform between base_footprint and map: <class 'tf2.LookupException'>: "sr1/map" passed to lookupTransform argument target_frame does not exist. 
[fleet_adapter.py-1] [INFO] [1738665742.107912369] [smartrover_command_handle]: Failed to update robot [sr1]: Unable to get transform between base_footprint and map
[fleet_adapter.py-1] [INFO] [1738665742.108196798] [smartrover_command_handle]: Failed to pose of robot [sr1]
[fleet_adapter.py-1] [INFO] [1738665742.209250458] [smartrover_fleet_adapter]: Adding robot [sr1] to fleet [smartrover].
[fleet_adapter.py-1] [ERROR] [1738665742.209469318] [smartrover_fleet_adapter]: Unable to compute a location on the navigation graph for robot [sr1] being added to fleet [smartrover] using map [L1] and position [108.278, 137.536, -2.916] specified in the initial_state argument. This can happen if the map in initial_state does not match any of the map names in the navigation graph supplied or if the position reported in the initial_state is far way from the navigation graph. This robot will not be added to the fleet.
[fleet_adapter.py-1] [INFO] [1738665742.308766563] [smartrover_fleet_adapter]: Adding robot [sr1] to fleet [smartrover].
[fleet_adapter.py-1] [ERROR] [1738665742.308894380] [smartrover_fleet_adapter]: Unable to compute a location on the navigation graph for robot [sr1] being added to fleet [smartrover] using map [L1] and position [108.270, 137.416, -2.942] specified in the initial_state argument. This can happen if the map in initial_state does not match any of the map names in the navigation graph supplied or if the position reported in the initial_state is far way from the navigation graph. This robot will not be added to the fleet.
[fleet_adapter.py-1] [INFO] [1738665742.409011998] [smartrover_fleet_adapter]: Adding robot [sr1] to fleet [smartrover].
[fleet_adapter.py-1] [ERROR] [1738665742.409119626] [smartrover_fleet_adapter]: Unable to compute a location on the navigation graph for robot [sr1] being added to fleet [smartrover] using map [L1] and position [108.276, 137.354, -2.956] specified in the initial_state argument. This can happen if the map in initial_state does not match any of the map names in the navigation graph supplied or if the position reported in the initial_state is far way from the navigation graph. This robot will not be added to the fleet.
[fleet_adapter.py-1] [INFO] [1738665742.512703974] [smartrover_fleet_adapter]: Adding robot [sr1] to fleet [smartrover].
[fleet_adapter.py-1] [ERROR] [1738665742.512974840] [smartrover_fleet_adapter]: Unable to compute a location on the navigation graph for robot [sr1] being added to fleet [smartrover] using map [L1] and position [108.344, 137.329, -2.924] specified in the initial_state argument. This can happen if the map in initial_state does not match any of the map names in the navigation graph supplied or if the position reported in the initial_state is far way from the navigation graph. This robot will not be added to the fleet.
[fleet_adapter.py-1] [INFO] [1738665742.617289731] [smartrover_fleet_adapter]: Adding robot [sr1] to fleet [smartrover].

Fleet adapter config file is as follows:

# FLEET CONFIG =================================================================
# RMF Fleet parameters

rmf_fleet:
  name: "smartrover"
  fleet_manager:
    ip: "127.0.0.1"
    port: 22011
    user: "some_user"
    password: "some_password"
  limits:
    linear: [0.5, 0.75] # velocity, acceleration
    angular: [0.6, 2.0] # velocity, acceleration
  profile: # Robot profile is modelled as a circle
    footprint: 0.3 # radius in m
    vicinity: 0.5 # radius in m
  reversible: True # whether robots in this fleet can reverse
  battery_system:
    voltage: 12.0 # V
    capacity: 24.0 # Ahr
    charging_current: 5.0 # AIDLE
  mechanical_system:
    mass: 20.0 # kg
    moment_of_inertia: 10.0 #kgm^2
    friction_coefficient: 0.22
  ambient_system:
    power: 20.0 # W
  tool_system:
    power: 0.0 # W
  recharge_threshold: 0.10 # Battery level below which robots in this fleet will not operate
  recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks
  publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency
  account_for_battery_drain: True
  task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing
    loop: True
    delivery: True
  # actions: ["some_action_here"]
  finishing_request: "charge" # [park, charge, nothing]
  responsive_wait: True # Should responsive wait be on/off for the whole fleet by default? False if not specified.
  robots:
    # tb1:
    #   # charger: "robot1_charger"
    #   responsive_wait: False # Should responsive wait be on/off for this specific robot? Overrides the fleet-wide setting.
    #   # For Nav2RobotAdapter
    #   navigation_stack: 2
    #   initial_map: "L1"
    #   maps:
    #     L1:
    #       map_url: "/opt/ros/humble/share/nav2_bringup/maps/turtlebot3_world.yaml"  
    sr1:
      charger: "charger"
      responsive_wait: False # Should responsive wait be on/off for this specific robot? Overrides the fleet-wide setting.
      # For Nav2RobotAdapter
      navigation_stack: 2
      initial_map: "L1"
      maps:
        L1:
          map_url: "/home/einfochips/my_map.yaml"
    
      # initial_pose: [109.138, 135.746, -2.940]

  robot_state_update_frequency: 10.0 # Hz


# TRANSFORM CONFIG =============================================================
# For computing transforms between Robot and RMF coordinate systems
reference_coordinates:
  L1:
    rmf: [[42.3770, 10.4130],
          [90.8730, 6.2020],
          [98.4800, 101.7010],
          [26.6190, 102.9240]
          ]
    robot: [[0.0848, 3.7000],
            [2.5200, 3.8600],
            [2.9100, -0.9010],
            [-0.7320, -0.9540]
            ]

[aaronchongth]

Hi @Anisha-thakkar! Are you certain that the reference coordinates are filled in accurately? Check out https://osrf.github.io/ros2multirobotbook/integration_fleets_adapter_tutorial.html#2-update-the-configyaml-file for some more information. If you're using a laser scan as a floor plan for the building map, you can set up vertices (corners of walls, pillars, etc) in traffic-editor to use for the RMF reference points, and within your own navigation stack check what the coordinates are for those points with respect to the robot.

The other thing to check would be to ensure that your robot's location is on a waypoint, or near it, or on a lane in the navigation graph.

If this issue persists, it would be easier to debug if you could share your .building.yaml and the corresponding scan

[Anisha-thakkar]

I have followed the instructions page and fleet adapter template page; I have used reference co-ordinates of 4 end of the map in building.yaml and rviz map respectively. I have also tried putting reference co-ordinates in pixels and also meters but did not work. I have not given any initial pose but it still throwing error like this so I am not able to find a root cause.
building.yaml
smartrover_map.building.zip

[Anisha-thakkar]

Hello @aaronchongth I have tried debugging it all possible ways, This error throws from this given snippet:

 robot_nav_params->find_stacked_vertices(graph);
  const Eigen::Vector3d position = *position_opt;
  auto starts = robot_nav_params->compute_plan_starts(
    graph, initial_state.map(), position, now);
  
  if (starts.empty())
  {
    const auto& p = position;
    RCLCPP_ERROR(
      node->get_logger(),
      "Unable to compute a location on the navigation graph for robot [%s] "
      "being added to fleet [%s] using map [%s] and position [%.3f, %.3f, %.3f] "
      "specified in the initial_state argument. This can happen if the map in "
      "initial_state does not match any of the map names in the navigation "
      "graph supplied or if the position reported in the initial_state is far "
      "way from the navigation graph. This robot will not be added to the "
      "fleet.",
      robot_name.c_str(),
      fleet_name.c_str(),
      initial_state.map().c_str(),
      p[0], p[1], p[2]);

    _pimpl->cmd_handles.erase(robot_name);
    return nullptr;
  }

I am not sure which parameter might be affecting this error.
There is one other thing I am concerned about, I am using cartographer for mapping on my robot, so is there any case that this might be an issue?

[aaronchongth]

Hi @Anisha-thakkar, I see that you've shared your building map, please share my_map.yaml and my_map.png as well if possible.

The error message is accurate, the issue is either

• the reference coordinates are wrong, or
• the robot is physically very far away from any waypoints, and RMF would consider it lost

Just to go over the process of finding these reference points again, the RMF reference coordinates are filled in using these from the traffic editor, in meters

Take note of where your selected location is, and check the corresponding same location physically with respect to your robot's map frame. Those coordinates are used to fill in the robot reference coordinates, in meters.

For example, if you selected a corner of a pillar in RMF traffic editor to use a reference coordinates. You will need to find the same pillar in the robot's map frame, and obtain the coordinates of that same corner, in meters and place it in the configuration file. Repeat this for 4 times, to obtain the 4 reference coordinates.

If you are running a Nav2 robot, you can start rviz during nav2 bringup, and use the Publish Point feature to click onto the map, and obtain the coordinates via ros2 topic echo /clicked_point

For context, this is the method used to populate the reference coordinates in the fleet configuration files in free_fleet_examples

[Anisha-thakkar]

Thank you for your reply.

my_map.zip
I will try to check reference point and initial location again.

[Anisha-thakkar]

Hello @aaronchongth I had two questions/ doubts regarding these issue.

1. In case of free_fleet_examples which is turtlebot_world example, in the turtlebot_world.building.yaml all the co-ordinates are mentioned in meters(m) and after colcon build even the nav graph 0.yaml has the co-ordinates in meters(m). While in my case building.yaml is created in meters and 0.yaml is generated in pixels. I am not sure about why is this happening. Though I have check with both the refrence points, In the the cases I am getting the same error. can this be root cause?
2. Is this rmf compute sync is dependent on the slam technique as I am using cartographer on my robot?

[aaronchongth]

Hey @Anisha-thakkar. You are missing a measurement of your map, to be defined in traffic editor. Please see https://osrf.github.io/ros2multirobotbook/traffic-editor.html#adding-a-measurement

Edit: you can compare how a measurement should appear in a building.yaml file after it has been defined, https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_maps/maps/office/office.building.yaml#L108

[Anisha-thakkar]

Thank you @aaronchongth for your help. Adding measurements to the building.yaml worked for me. Now the robot is successfully added to the fleet and able to visualize in rmf-web too, I am still not able to give navigation task from open-rmf server, so trying to debug that, seems to be a communication issue.

[aaronchongth]

Good to hear that. Please try sending via ros2 run rmf_demos_tasks dispatch_patrol ..... too, to isolate which parts are having communication issues. Please ensure that the command is run using the same RMW and ROS domain ID

edit: please also mark the reply as the answer for this discussion, so it helps other folks with the same issue.

[Anisha-thakkar]

I am running with same RMW Implementation and ROS domain ID, here while I give task either through command line ros2 run rmf_demo_tasks... or through rmf-web, it successfully pass the task through json, it found the path and try to apply navigate to pose but there is error coming shown below, I have tried debug it and derived that navigate_to_pose action server is not working properly, so the feedback coming from it reply.ok value is 'none' and that is why replay.ok.payload is throwing the following error.

Logs of fleet_adapter:

[fleet_adapter.py-1] [INFO] [1739947300.786791943] [smartrover_fleet_adapter]: Beginning new task [patrol.dispatch-8dca22a2cd] for [smartrover/sr1]. Remaining queue size: 0
[fleet_adapter.py-1] [INFO] [1739947300.787030954] [smartrover_fleet_adapter]: TaskAssignments updated for robots in fleet [smartrover] to accommodate task_id [patrol.dispatch-8dca22a2cd]
[fleet_adapter.py-1] [INFO] [1739947300.789948604] [smartrover_fleet_adapter]: Executing go_to_place [charger] for robot [smartrover/sr1]
[fleet_adapter.py-1] [INFO] [1739947300.797696575] [smartrover_command_handle]: Commanding [sr1] to navigate to [ 1.10640753 -0.00492009  1.51899317] on map [L1]
[fleet_adapter.py-1] [ERROR] [1739947310.808882382] [smartrover_command_handle]: Received (ERROR: Timeout: <class 'AttributeError'>: 'NoneType' object has no attribute 'payload')
[fleet_adapter.py-1] [INFO] [1739947310.809618569] [smartrover_fleet_adapter]: Requesting new schedule update because update timed out
[fleet_adapter.py-1] [INFO] [1739947310.809687995] [smartrover_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since version [10]
[fleet_adapter.py-1] [WARN] [1739947310.815260342] [smartrover_fleet_adapter]: Requesting replan for [smartrover/sr1] because its command handle seems to be unresponsive
[fleet_adapter.py-1] [INFO] [1739947310.818928225] [smartrover_fleet_adapter]: Replanning requested for [smartrover/sr1]
[fleet_adapter.py-1] [INFO] [1739947310.819991517] [smartrover_fleet_adapter]: Planning for [smartrover/sr1] to [charger] from one of these locations:
[fleet_adapter.py-1]  -- lane 3: { L1 < 1.95426 -4.18347> [initial] } -> { L1 < 3.11347 -4.16117> [b1] } | location < 2.03054 -4.19563> | orientation 1.56912
[fleet_adapter.py-1]  -- lane 2: { L1 < 3.11347 -4.16117> [b1] } -> { L1 < 1.95426 -4.18347> [initial] } | location < 2.03054 -4.19563> | orientation 1.56912
[fleet_adapter.py-1] [INFO] [1739947310.832548138] [smartrover_fleet_adapter]: Executing go_to_place [charger] for robot [smartrover/sr1]
[fleet_adapter.py-1] [INFO] [1739947310.837172111] [smartrover_command_handle]: Commanding [sr1] to navigate to [ 1.10640753 -0.00492009  1.51899317] on map [L1]
[fleet_adapter.py-1] [ERROR] [1739947320.841318658] [smartrover_command_handle]: Received (ERROR: Timeout: <class 'AttributeError'>: 'NoneType' object has no attribute 'payload')
[fleet_adapter.py-1] [WARN] [1739947320.847131270] [smartrover_fleet_adapter]: Requesting replan for [smartrover/sr1] because its command handle seems to be unresponsive
[fleet_adapter.py-1] [WARN] [1739947320.847455713] [smartrover_fleet_adapter]: Failed to update using patch for DB version 72 (mirror version: none, patch base: 12); requesting new update
[fleet_adapter.py-1] [INFO] [1739947320.847557902] [smartrover_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since beginning of recorded history
[fleet_adapter.py-1] [INFO] [1739947320.851026725] [smartrover_fleet_adapter]: Replanning requested for [smartrover/sr1]
[fleet_adapter.py-1] [INFO] [1739947320.851424634] [smartrover_fleet_adapter]: Planning for [smartrover/sr1] to [charger] from one of these locations:
[fleet_adapter.py-1]  -- lane 3: { L1 < 1.95426 -4.18347> [initial] } -> { L1 < 3.11347 -4.16117> [b1] } | location < 2.03281 -4.19859> | orientation 0.575832
[fleet_adapter.py-1]  -- lane 2: { L1 < 3.11347 -4.16117> [b1] } -> { L1 < 1.95426 -4.18347> [initial] } | location < 2.03281 -4.19859> | orientation 0.575832
[fleet_adapter.py-1] [INFO] [1739947320.864997817] [smartrover_fleet_adapter]: Executing go_to_place [charger] for robot [smartrover/sr1]

Logs of running nav2_send_navigate_to_pose.py:

ros2 run free_fleet_examples nav2_send_navigate_to_pose.py     --frame-id map     --namespace sr1     -x 1.808     -y 0.503
[110, 70, 181, 243, 105, 141, 71, 217, 96, 104, 115, 0, 167, 246, 148, 151]
Sending goal
Reply was not ok!
Goal accepted by server, waiting for result
None
'NoneType' object has no attribute 'payload'
Received (ERROR: 'Timeout')
None
'NoneType' object has no attribute 'payload'
Received (ERROR: 'Timeout')

[aaronchongth]

Hi @Anisha-thakkar. The replies and feedback come from the sr1 robot's navigation stack, and my guess is that it operates differently from the navigation2 example stack on a tb3. In that case, you will need to debug with your robot's navigation stack, and figure out what is the setup difference between your robot and the basic nav2 example tb3. I will suggest SSH-ing into your robot, and publishing ROS 2 actions to ensure it works.

Hope you find the issue and resolve it! Unfortunately, this robot-specific debugging is out of the scope of RMF and free fleet, and we might not be able to help much.

[Anisha-thakkar]

Thank you for co-operating. I have checked the navigate_to_pose action it was not working on the robot and solved that issue, but the error still persists and through debugging I have found out it is not an issue from either robot side or rmf side, Zenoh bridge is throwing the error so I will try to solve it.
Thank you for your help.