Route Server Improvements to Support Dynamic Graphs/Operations

[e1d1s1]

I am investigating ways of implementing fleet management, VDA5050 in particular, within an industrial settings. A majority of the time, if the fleet manager does not provide a prescribed path, we will be behaving as if we were a "line-following" robot following paths between nodes in a manner very similar to the spline paths generated by ComputeRoute via the route server. Certainly, I could leverage standard bt_navigator functionality via NavigateThroughPoses directly with some kind of spline planner, and track progress via some kind of hard-coded state machine and halt for various "blocking" actions like manipulating/picking/dropping objects in the facility. However, the route server "feels" very much like a possible solution.

The "dynamic" issue is we also have to contend with upstream fleet traffic management via the notion of the node "horizon (an arbitrary node along the route before the final goal we cannot yet pass beyond)" vs the final node "goal", with the "horizon" being updated in subsequent order updates, with the expectation we will "wait" at the horizon once we reach it and wait for more nodes up to the goal to be "released". We also have to contend with the fact that an order update could come in during the path follow that may append another node to the graph, or any nodes in the order update beyond the current "horizon" may have new/updated actions to perform.

The Route Server is very "static" in its behavior. I could generate a geoJSON file from the initial order, but I would still have to contend with order updates.

Some feature ideas for consideration:

1. Would a new service "AppendToRouteGraph" for the route server make sense for adding new RouteNodes to the graph after initial load?
2. Would a new service "UpdateNodeMetadata" for the route server in order to expose the internal Node meatadata make sense so we could inject "actions" into the OperationsManager for the graph after initial load?
3. If a ComputeAndTrackRoute is in progress and we want to extend the final goal to a newly appended node, an "UpdateRoute" service might make sense, too.

I could envision a fully "reactive" event system this way:

• Once ComputeAndTrackRoute begins, the first node would have a "navigate to next blocking action or horizon" event associated with it triggered by the OperationsManager. So that really could be a call to bt_navigator using the BT "Navigate on Route Graph with Recovery" from the current node ID up to the first blocking action or horizon node ID.
• Perform necessary blocking action via OperationsManager
• repeat until Route complete.

We just need to contend with the fact that the Route length could be extended after initial graph load, and while we are still tracking.

Thoughts?

[SteveMacenski]

If you have the route edges/nodes from your fleet manager, why do you need the Route Server? Simply use those and the preemption commands with the new unlocked nodes/edges as they are given to a particular robot to continue on its mission. This seems very straight forward. I don't see why you'd involve the route server if your fleet manager is essentially acting as the route server to provide the nodes and edges to traverse (except if you want to use it to perform actions at each; then I can understand but the issue becomes less using the route planner and more using the route behaviors).

The update / 'dynamic' changes are trivial to handle in the BT / FollowPath. These are not unique or difficult.

---

Certainly it could be valuable to allow the route graph to be dynamically adjusted in the Route Server, but I question if that's really what you want in such case. It sounds like you just want to call the navigator with your new set of nodes/edges when new ones are unlocked and update paths generated by those nodes/edges to send to the controller server as a path update.

---

If you did want to use the route server because you were embedding metadata for actions along the route set by your fleet manager -- then what seems most sensible to me is to create a new action API in the Route Server that when provided a precomputed route to use that to track and action the operations against (rather than computing it itself given start/goal information). That should be pretty easy to do and would be a welcome pull request. Then in your BT you use that from your given new fleet manager derived route.

Would that not better solve your needs?

[e1d1s1]

It certainly is the OperationsManager and associated fleet->route_server metadata data transfer that makes the idea attractive. Also that fact that VDA5050 allows the manager to pass nodes by only IDs in some cases as positional data is "optional", and I am not entirely sure if we will get paths/curves, but we will be expected to stay in "lanes". The desire for executing non-navigational tasks at specific poses along a long navigational task I think is also the impetus behind the "Operation Server" proposal I saw here: #6002

What logically lines up is a fleet manger provides us an "order" for which it wants feedback, whereas ComputeAndTrack can provide feedback over the entire lifetime of A -> B -> C , which aligns with the lifetime of the order processing. On the course of traversing a long running order A --> B (action) -> C ComputeAndTrack is fairly oblivious to whatever B does and just keeps providing progress feedback of the entire order/job as a single unit. We just have to create an Operations plugin to wrap "B". If "C" was not yet "released" at the initial order, the robot will naturally wait at "B' with no further movement actions assigned to that node.

I suppose adding a new node either to bt_navigator or route server requires a kind of preemption, but its much more "silent" in the route server. In the route server, I just perform a node update on "C" to "release" it, by attaching an additional navigation task on "B" when the order update says to do so via "releasing" "C".

I added these services to route_server:
AppendRouteGraph.srv

std_msgs/Header header

# Nodes to add to the graph. You must manage ids. Collisions will result in failure.
nav2_msgs/RouteNode[] nodes

# A json object representing similar key/value that would be defined in geoJSON node operations
string[] node_operations_json

# A json object representing similar key/value that would be defined in geoJSON metadata
string[] node_metadata_json

# Edges to add to the graph. You must manage ids. Collisions will result in failure. 
nav2_msgs/RouteEdge[] edges

# Cost information for the edges
nav2_msgs/EdgeCost[] edge_costs

# Cost override flag for edge
bool[] edge_costs_overridable

# A json object representing similar key/value that would be defined in geoJSON edge operations
string[] edge_operations_json

# A json object representing similar key/value that would be defined in geoJSON metadata
string[] edge_metadata_json
---
bool success

UpdateRouteGraph

# node ids to update
uint16[] nodeids

# new or replacement operations json data. Should be a json key-value pair.
# Existing keys will be overwritten with this update.
string[] node_operations_json

# new or replacement metadata json data. Should be a json key-value pair.
# Existing keys will be overwritten with this update.
string[] node_metadata_json

uint16[] edgeids

# new or replacement operations json data. Should be a json key-value pair.
# Existing keys will be overwritten with this update.
string[] edge_operations_json

# new or replacement metadata json data. Should be a json key-value pair.
# Existing keys will be overwritten with this update.
string[] edge_metadata_json
---
bool success

ExtendRoute

std_msgs/Header header

# node ids to append to the currently executing ComputeAndTrack Route
uint16[] append_node_ids

# edge ids to append to the current executing ComputeAndTrack Route
uint16[] append_edge_ids
---
bool success

I created a couple of operations plugins in my prototype:

1. I created nav2_route_plugin for my action "B".

2. I created a "FollowRoute" nav2_route_plugin, that internally just calls ComputeRoute to the next "blocking" action in the order and passes the resulting path to the nav2 controller. I could do the same with NavigateThroughPoses if "free-space" navigation was allowed to the next node(s). So in practice, once the robot is within the radius of "A", it immediately reacts by performing a follow to B.

As such, once the robot touches A, it behaves like a self-propelling system controlled entirely via the route server and operation manager until the robot reaches the final order goal. The operation style is almost board game-like, as if the order/graph is the monopoly board, nodes are cells on the board, upon which some trigger special behaviors (action B "go to jail").

---

As an aside, one improvement I think that needs to be made to route server is the RouteEdge message needs to track the start and end nodeid. Right now the definition is:

uint16 edgeid
geometry_msgs/Point start
geometry_msgs/Point end

Unless the idea was to for some reason stringing consecutive edges together without nodes, it makes more sense to store node start/end ids. Easier to track that way, and thats the way it ends up stored in geoJSON format anyways.

[e1d1s1]

After sleeping on the implementation, I am questioning whether it makes sense to expend on the idea of non-navigational tasking within a navigational framework. The operations manager certainly is a useful idea, and I'll probably carry forward something similar in my own "mission client" implementation.

Thanks for the feedback.

[SteveMacenski]

I'm not entirely following your train of thought from a first glance, but sounds like you decided on another direction.

I think you can do everything you want in this framework if we simply add an action to the Route Server that gives a provided route from the fleet manager and letting the rest of the tracking / handling go as usual. The controller server only needs to know about the next edge(s) that are free (from the fleet manager or otherwise pruned to the locked region it can travel within) to handle the deconfliction problem easily.

I don't think anything related to appending / updating / extending the route is needed or sensible. Just a new TrackPrecomputedRoute action that provides the route graph nodes/edges in order to track progression on for the use of tracking and operation triggering. The BT XML would handle passing the respective valid nodes/edges paths to the controller server to execute / stop when necessary. This seems pretty straight forward from my perspective.