Add support for multiple destinations to choose from.

[arjo129]

New feature implementation

Implemented feature

Implementation description

[Yadunund]

Whats the use case for this?

Having the model always return the first destination does not sound accurate unless the list of destinations is always sorted by distance. But for doing that you'll either need the Graph information or rely on some heuristic such as Euclidean distance from current state of the robot.

[arjo129]

@Yadunund, ideally, we would only use this feature in a cancellation phase, hence the model should not matter. An example of that would be if we were to cancel a delivery and want the robot to go to the nearest drop-off spot to drop off a cart. There isn't any API to enforce that down stream and the work required would be a fair bit more.

The reason I've kept the 0th index as the model is that it allows us to preserve the old behavior. I can put a note in the code or file an issue to remind us to change this. I don't think it'll be impossible to estimate the nearest location given the last location, if you think thats a good idea I can update this PR to do so tomorrow.

I've already had a chat with @mxgrey about this offline.

[Yadunund]

@Yadunund, ideally, we would only use this feature in a cancellation phase, hence the model should not matter. An example of that would be if we were to cancel a delivery and want the robot to go to the nearest drop-off spot to drop off a cart. There isn't any API to enforce that down stream and the work required would be a fair bit more.

The reason I've kept the 0th index as the model is that it allows us to preserve the old behavior. I can put a note in the code or file an issue to remind us to change this. I don't think it'll be impossible to estimate the nearest location given the last location, if you think thats a good idea I can update this PR to do so tomorrow.

I've already had a chat with @mxgrey about this offline.

I see thanks for explaining that. It still feels like a hack to make an existing event work for the required cancellation behavior especially since the behavior of going to the nearest waypoint among the list is not apparent from the description of the event.

What do you think about creating a new event, GoToNearest. The schema here and potentially in the rmf_fleet_adapter side, will allow values to be certain strings: waypoint, charger_waypoint, passthrough_waypoint, 'dropoff_waypoint`, etc. ie a string for whatever waypoint property we support in the traffic editor. When making the model, find the nearest suitable waypoint from the graph. But we'll need to potentially override the decision of which waypoint to go to when generating the runtime for the this event in rmf_fleet_adapter. Ie, recompute the decision would while factor in latest available information information such as closed lanes or traffic congestions. It could also rely on the reservation system to find the most available such waypoint.

If that is too much effort for short term, I think it would still be nice to have a GoToNearest event which accepts a list of waypoints and sorts then based on estimate_duration() like you did here 10badd1. The runtime for this event is still the same as a GoToPlace in rmf_fleet_adapter.

But if @mxgrey feels the existing approach is okay, then let's go ahead.

[arjo129]

What do you think about creating a new event, GoToNearest. The schema here and potentially in the rmf_fleet_adapter side, will allow values to be certain strings: waypoint, charger_waypoint, passthrough_waypoint, 'dropoff_waypoint`, etc. ie a string for whatever waypoint property we support in the traffic editor. When making the model, find the nearest suitable waypoint from the graph. But we'll need to potentially override the decision of which waypoint to go to when generating the runtime for the this event in rmf_fleet_adapter. Ie, recompute the decision would while factor in latest available information information such as closed lanes or traffic congestions. It could also rely on the reservation system to find the most available such waypoint.

This is kind of what we are aiming for. In my mind the use of the reservation system will be implicit. I think grey wants to keep the reservation system for next-gen RMF but I could be wrong. If you see the schema downstream in rmf_Fleet_adapters you can add contraints. That is not currently part of this event.

I actually don't see a reason for creating a new event as we'd have to maintain two versions of similar logic. Ultimately people could just use GoToNearestPlace to achieve the same effect as GoToPlace. Eventually we may even see a version with Strategy involved. For instance we may be interested in going to the farthest waypoint or some other criteria.

And yeah this is a hack.

[mxgrey]

Yeah I think GoToPlace is a broad enough term that it can encapsulate "Go to a specific place" or "Go to the nearest place out of a list of options" or in the future even "Go to the nearest place that belongs to a category (parking spot / charger)".

[Yadunund]

@arjo129 This line causes a segfault when goals is empty.