search

open-rmf / rmf

Root repository for the RMF software
Apache License 2.0
225 stars 58 forks source link

RMF fleet adapter does not read level_name from /fleet_states messages #135

Closed webvenky closed 2 years ago

webvenky commented 2 years ago

I use free_fleet_server_ros2 node to publish the /fleet_states. It looks like below:

---
name: hospi_robot
robots:
- name: hospi_robot_0
  model: robot_model
  task_id: ''
  seq: 0
  mode:
    mode: 0
    mode_request_id: 0
  battery_percent: 99.0
  location:
    t:
      sec: 13
      nanosec: 396000000
    x: 58.44990158081055
    y: -184.5000762939453
    yaw: -2.351127862930298
    level_name: B1
    index: 0
  path: []
---

But I get this message from the full_control fleet adapter, when I run the simulation:

[full_control-1] [ERROR] [1643943567.067598981] [hospi_robot_fleet_adapter]: Unable to compute a StartSet for robot [hospi_robot_0] using level_name [] and location [58.449944, -184.500015, -2.350461] specified in its RobotState message. This can happen if the level_name in the RobotState message does not match any of the map names in the navigation graph supplied or if the location reported in the RobotState message is far way from the navigation graph. This robot will not be added to the fleet [hospi_robot]. The following hint may help with debugging: None of the waypoints in the graph are on a map called [].

I checked the nav_graph/0.yaml and it has B1 level. The fleet adapter is able to get the robot_name and pose: x,y,yaw information correctly though.

Do I need to publish any other details on the fleet_states message?

mxgrey commented 2 years ago

That's certainly an odd issue.

There are a few things about your description that I'm not clear on. How are you setting up the simulation? Are you using the standard RMF "slotcar" plugin to simulate the robot, or are you generating robot states out of your simulation some other way?

For the standard slotcar plugin we have this block that prevents potential race conditions that may happen if the simulation is up and running before it gets any messages from the building map server. If you're not using the standard slotcar plugin then you might need something similar.

I see that your fleet state contains level_name field, but (for better or worse) the current design of the full_control fleet adapter will permanently block any robot that sends an initial state with an invalid level_name value. Even if you're publishing valid states later, it won't accept those if the first state it received was an invalid one. We could make this less strict, but we find it's helpful for identifying system integration mistakes as early as possible.

webvenky commented 2 years ago

Sorry. I realized I was inadvertently using an old version of the rmf_fleet_msgs package in one of the workspaces and the message definition had changed for Location.msg. I fixed it and is now able to run. Thanks for your time @mxgrey .