Navigation Graph Interpretation

[txlei]

1. Could I understand more on the format of the navigation graph generated by traffic editor and used by fleet adapter API? Referred to ParseGraph.cpp but still can't interpret how the the lanes and vertices information are used. Could you give an overview and also specifically I have a qn below:

• for lanes, the start and end of each lane are not tagged with location information. The number of items in the lanes list and vertices list does not match. So where is this location information be available?

2. I believe that there is a piece of code in the fleet_adapter library that identifies the robot's current location from /fleet_states on its navigation graph. i.e. which lane does the robot occupies. Could you point me to the related files for reference?

[mxgrey]

After calling the parse_graph function, you will get back a rmf_traffic::agv::Graph instance, so I'm going to refer to the API of rmf_traffic::agv::Graph (from now on I'll just refer to it as Graph) to explain the graph structure. The yaml format is just an intermediate disk format, so there's not much need to understand it.

The Graph API has two important nested classes: Graph::Waypoint and Graph::Lane.

Graph::Waypoint class ties the graph to its real-world locations using Graph::Waypoint::get_map_name() and Graph::Waypoint::get_location().

The Graph::Lane class connects waypoints by defining an entry and exit node. Each node specifies a waypoint_index. You can pass that waypoint_index into Graph::get_waypoint(~) to see information about the entry/exit waypoint of the lane (such as the waypoint's position).

Each lane is uni-directional. To put a bi-directional edge into the graph, you simply create two lanes where their entry/exit waypoints are the reverse of each other.

I believe that there is a piece of code in the fleet_adapter library that identifies the robot's current location from /fleet_states on its navigation graph. i.e. which lane does the robot occupies. Could you point me to the related files for reference?

I think what you're looking for is the compute_plan_starts function. I would urge caution when using this function, since it's not hard for it to get confused about what lane the robot is actually on. This function is the leading cause of this problem, so we're trying to move away from depending on it as much as possible. If you use that function to pick which lanes to close, you may accidentally end up closing other lanes that you didn't mean to.

If your goal is to identify which lane is being blocked when a robot gets stuck, then instead of using compute_plan_starts, I might recommend keeping track of which two waypoints the robot is moving between and then calling Graph::lane_from(from, to) to find which lane is actually being blocked.

[txlei]

keeping track of which two waypoints the robot is moving between

1. I plan to get the Location[] from /robot_path_request topic and the current location of the robot from /fleet_states, and then compute which two waypoints are the closest to the robot. Do you agree with this? I observe that the path list in /robot_path_request seem to always have repeated elements. Would not be a big problem but like to understand why.

2. Also, lets say if there are no tasks requests and for some reason I would like to disable a traffic lane. Is there another approach to identify the correct lane?

3. Is there a library that maps the nav_graph to their respective fleet_name or robot_name?

[mxgrey]

I plan to get the Location[] from /robot_path_request topic and the current location of the robot from /fleet_states, and then compute which two waypoints are the closest to the robot. Do you agree with this?

For implementing this in the simulation, this may be the simplest approach that doesn't require any changes to the full_control fleet adapter. I think it was mentioned that this would ultimately be implemented on a ROS-based robot, in which case you'll probably want to use free_fleet, which operates a bit differently than the out-of-the-box simulation fleet adapter does. @aaronchongth: Maybe we can add some feature to free_fleet that publishes the index of the robot's current lane?

I observe that the path list in /robot_path_request seem to always have repeated elements. Would not be a big problem but like to understand why.

Are the elements exact duplicates? I would expect (x, y) coordinates to repeat sometimes, but the yaw value should be different between them. That's the fleet adapter's way of saying "Go to this position and then turn".

Also, lets say if there are no tasks requests and for some reason I would like to disable a traffic lane. Is there another approach to identify the correct lane?

• If the fleet adapter is commanding the robot to move, then it will be issuing commands over the robot_path_request topic, regardless of whether it is obeying a task request or carrying out the ResponsiveWait behavior.
• If the robot is moving completely on its own, not commanded by the fleet adapter, then you would need to get the relevant lane information from whoever is commanding it.
• If the robot is not moving at all but you are aware of some obstacle on the map which you think warrants closing the lane, then I guess you would need to look at the Graph information and figure out which two connected waypoints are disrupted by the obstacle.

Is there a library that maps the nav_graph to their respective fleet_name or robot_name?

It's the job of the system integrator to know which nav graph belongs to which fleet. We're open to ideas on how to make this easier. One thought I've had is that we could have full control fleet adapters broadcast their navigation graph over a topic. That would provide a single source of truth for the navigation graphs. That feature does not currently exist, though.

[txlei]

hi @mxgrey I am unsure how to get the waypoint index from its vector2d location. Graph class does not provide that method. eg. graph::get_index(Eigen::Vector2d location). Do I have to use this rmf_traffic::agv:Plan::Waypoint ::graph_index()?

which operates a bit differently than the out-of-the-box simulation fleet adapter does

Could you specify the difference? I thought that regardless whether a fleet adapter interfaces with free fleet, an existing fleet manager, or the slotcar plugin, the defined message flow by fleet adapter are the same. eg. still using PathRequest to communicate.

yaw value should be different between them

yes, you are right, the yaw values are different.

which two connected waypoints are disrupted by the obstacle.

yeap, I am referring to the last scenario. As mentioned, the compute_plan_starts from the Planner class might not be accurate. Do you mean creating an alternative approach that does the equivalent?

[mxgrey]

I am unsure how to get the waypoint index from its vector2d location

Given a std::string map, Eigen::Vector2d coordinates and a Graph graph, you can do an O(N) search through the graph to find a matching waypoint. The for loop would look something like

for (std::size_t i=0; i < graph->num_waypoints(); ++i)
{
  const auto& waypoint = graph->get_waypoint(i);
  if (waypoint->get_map_name() != map)
    continue;

  // Check if waypoint->get_location() matches the input coordinates
  // If it's a match, then waypoint i is the desired waypoint index
}

The catch with this approach is that sometimes the coordinates in the path following request do not belong to a specific waypoint. For example, if the robot gets stopped in the middle of a lane because of a conflict, then the initial coordinates in path following request will be in the middle of a lane. If you try to look up a waypoint whose location matches those coordinates, you need to handle the possibility that no waypoint will match. If you choose the closest waypoint instead of a perfect match, then you have to deal with the possibility that you'll be choosing the wrong waypoint.

Do I have to use this rmf_traffic::agv:Plan::Waypoint::graph_index()?

You'll only have access to that function if you're writing a fleet adapter using the C++ API, but yes, that would be the most reliable way to identify the waypoint indices.

Could you specify the difference? ... eg. still using PathRequest to communicate.

No, the PathRequest message is part of an old API that new users are discouraged from using. Instead you should either use the free_fleet to integrate your robot (which has been able to run on at least two ROS1-based robots that we know of), or you should create your own fleet adapter using the C++ API that's described here.

yeap, I am referring to the last scenario

In this case, I recommend iterating through each lane in the graph and testing whether the path between the lane's entry and exit waypoint is intersected by the obstacle. If you are able to know the coordinates of the obstacle, then I strongly recommend just taking this approach for the whole problem of identifying which lanes to close. I think this approach will be much more reliable than trying to infer which lane the robot is on whenever you notice that it is trying to reroute itself.

Another detail that occurred to me: You'll want to think about how/when to reopen the lanes that you have closed. Otherwise over time all the lanes will be closed down and then the robots won't know how to get anywhere.