osrf / rmf_core

Provides the centralized functions of RMF: scheduling, etc.
Apache License 2.0
102 stars 41 forks source link

proposal for traffic negotiation #189

Closed txlei closed 3 years ago

txlei commented 3 years ago

the multirobotbook mentioned this: Each fleet manager will submit its preferred itineraries, and each will respond with itineraries that can accommodate the others.

and I am confused because what I know is that most agv's path planning only relies on shortest path algo and would result in only one trajectory to move to destination. which is a limitation and where rmf traffic comes handy to see what are the alternative paths the agv can take when conflict arises regardless of its mission.

I think, high-level wise, what I like to know is at the start of a traffic negotiation, who proposes a new itinerary to handle traffic conflict?

lastly, could you explain more about the concept of the greedy plan?

mxgrey commented 3 years ago

I think, high-level wise, what I like to know is at the start of a traffic negotiation, who proposes a new itinerary to handle traffic conflict?

Since the RMF protocol is brand new, there naturally aren't any fleet managers in existence right now that know how to participate in the RMF traffic negotiation on their own. Such a fleet manager could be made using the rmf_traffic tools (or it could be made from scratch, although that's not recommended), but no one has done that yet.

Instead, we use a "fleet adapter" layer to bridge the RMF traffic negotiation API and each robot fleet's unique API. There are three categories of fleet adapter that we provide out of the box, as described here (full control, traffic light, and read-only).

Each of these fleet adapter categories uses different strategies when negotiating.

  1. The "full control" category knows everything that the underlying robot can do, so the fleet adapter will compute the best possible itinerary given the current schedule and the other robot's negotiation submissions. When the negotiation is complete, the fleet adapter will send an explicit command to the underlying fleet API to follow the path that was decided on by the negotiation.

  2. The "traffic light" category only knows a single path that its robot can follow, so the fleet adapter will only try to find the best possible pause/resume timing along the path that it's been given. When the negotiation is complete, the fleet adapter will use the underlying fleet API to issue pause and resume commands based on the timing that was decided on by the negotiation.

  3. The "read only" category can't actually control the robot at all, so the fleet adapter will just declare what its robot's path is and reject any proposals that conflict with that path.

lastly, could you explain more about the concept of the greedy plan?

The greedy plan is very simple. It's just a plan that gets made while completely ignoring the traffic schedule. The plan that gets produced will be the best possible plan for the robot, but it may have conflicts with other traffic participants. Generating a greedy plan may be useful even if it has conflicts, for the following use-cases:

  1. A robot is trapped by other traffic participants (meaning it has no way to get to its goal because other participants are blocking it), so it can generate a greedy plan, knowing that it will create a conflict and begin a negotiation. The negotiation can then be used to effectively ask the other robots to move out of its way.

  2. If a robot tries to accommodate all the other robots in the schedule, it might have to suffer too much of a delay in completing its task. It can submit a greedy itinerary knowing that it will create conflicts, and then maybe it can get a better plan out of the negotiation process.

txlei commented 3 years ago

thanks for the quick response.

assuming full-control level for fleet adapter for all of the following

  1. Is the global path planner from robot's own navigation stack used for the very first version of schedule itinerary? eg. upon a delivery request and before any conflict is detected
  2. which class handles the functionality where "fleet adapter will compute the best possible itinerary given the current schedule and the other robot's negotiation submissions". referring to this PR, ive read and found the Negotiation service and its implementation files, but don't understand which part lies the logic of choosing the most optimal itinerary based on other fleet's proposed itinerary provided in rmf_traffic::schedule::Negotiator::TableViewerPt.
  3. the best possible itinerary is using the A search algo? or Is the A search algo just used to detect conflicts in rmf_traffic and not involved at all in rmf_fleet_adapter?
  4. during the "prevention" stage (i.e. before actual conflict happens but a possible conflict is detected), does traffic negotiation happens? does it happens immediately even before scheduled delivery time?
  5. (linked to 4) also if it does, how does the proposed itinerary decide whether to vary speed of robot or completely change its path to destination, as part of conflict resolution?
  6. Since there is already a fleet adapter Negotiation implementation in rmf_core repo, can that portion be used for a proprietary standard AGV/ AMR? even if the fleet manager API is different.
  7. From this illustration, "smart fleet adapter" is not part of "the core system". Is "smart fleet adapter" shown in the diagram equivalent to rmf_core/rmf_fleet_adapter package? I am interpreting "the core system" is rmf_core repo.
  8. from my understanding looking at rmf_core/rmf_fleet_adapter package it contains 3 main portions:
    • provides fleet adapter API, fleet manager API and integration piece (reference to diagram in #168)
    • negotiation portion to work with rmf_traffic package
    • supervisors to facilitate infra usage Is this correct? assuming my environment and requirements is the same in rmf_demo, can the last 2 points be recycled for other implementation of a fleet adapter? on the flip side, to create a new fleet adapter for my robot, do I just need to work on the first point?

dropping many qns but I think I'm getting there in terms of understanding rmf. really appreciate your help!

mxgrey commented 3 years ago

Is the global path planner from robot's own navigation stack used for the very first version of schedule itinerary? eg. upon a delivery request and before any conflict is detected

Nope, for the current implementation of the out-of-the-box full-control fleet adapter, the robot's own navigation stack is only used for local planning, not for coming up with the global plan.

which class handles the functionality where "fleet adapter will compute the best possible itinerary given the current schedule and the other robot's negotiation submissions"

There isn't a single class that does this alone. The minimal classes involved in computing a negotiation proposal are:

Combining those three classes allows you to generate an optimal plan in response to the proposals that have been put forward by other vehicles in the negotiation.

the best possible itinerary is using the A* search algo?

Yes, it's essentially an A* search algorithm, but beefed up a bit to deal with the time-variant nature of the problem.

during the "prevention" stage (i.e. before actual conflict happens but a possible conflict is detected), does traffic negotiation happens? does it happens immediately even before scheduled delivery time?

Yes, as soon as a conflict is predicted based on all the robots' scheduled itineraries, the conflict will be announced and a negotiation will start. Ideally every negotiation should be settled long before the robots actually experience any conflict. If all goes well, then an outside observer might never realize that there was ever a risk of conflicts at all, because the robots will simply accommodate each other optimally.

(linked to 4) also if it does, how does the proposed itinerary decide whether to vary speed of robot or completely change its path to destination, as part of conflict resolution?

This just naturally falls out of the A planner optimally. At every waypoint on the navigation graph (except waypoints marked as passthrough), the A planner is allowed to have the robot sit and wait. The planner will determine what sequence of choices (including the choice to sit and wait) provides an optimal solution, because ... well, it's A*.

Since there is already a fleet adapter Negotiation implementation in rmf_core repo, can that portion be used for a proprietary standard AGV/ AMR? even if the fleet manager API is different.

Yes, everything in this rmf_core repo has an Apache 2.0 License, so it's all free to be used for any purpose, including being used by proprietary software or even copied into proprietary software codebases. The only requirement for someone who is copying the source code is that they also copy the text of the Apache 2.0 License for the portions of source code that were copied. But the Apache 2.0 License will still only apply specifically to the copied pieces of code; you can still modify the code however you like and keep your modifications closed source and proprietary. Only people who have access to your closed off source code will be able to even see the Apache 2.0 License at that point. I know licensing gets kind of confusing, that can be discussed further if it's still unclear.

From this illustration, "smart fleet adapter" is not part of "the core system". Is "smart fleet adapter" shown in the diagram equivalent to rmf_core/rmf_fleet_adapter package? I am interpreting "the core system" is rmf_core repo.

There are many iterations and variations on the RMF/RoMi-H diagram floating around. I wouldn't worry too much about the specifics in any of them; they're just meant to give a quick conceptual overview. We'll try to come up with a very precise and accurate diagram when our development effort isn't as hot and time allows us to.

from my understanding looking at rmf_core/rmf_fleet_adapter package it contains 3 main portions:

provides fleet adapter API, fleet manager API and integration piece

Only the "Fleet Adapter API" is provided by the rmf_fleet_adapter package. I understand the naming is confusing; it wound up like this for legacy reasons, and we may try to rename this package in the future. The "Fleet Adapter API" needs to be combined with a "Fleet Manager API" and an "Integration Piece" to form a full "Fleet Adapter". But the "Fleet Manager API" and the "Integration Piece" need to be supplied by the user.

Your other two bullet points are correct.

assuming my environment and requirements is the same in rmf_demo, can the last 2 points be recycled for other implementation of a fleet adapter? on the flip side, to create a new fleet adapter for my robot, do I just need to work on the first point?

Yes and yes. Truthfully the infrastructure supervisors should probably go in their own package since they don't really have anything to do with developing a fleet adapter; they're just in the rmf_fleet_adapter package for legacy reasons.

dropping many qns but I think I'm getting there in terms of understanding rmf.

I can tell from the questions that you're asking that you're getting a great handle on how it all works. You'll be an RMF expert in no time :+1:

txlei commented 3 years ago

nice. lastly, for this

which class handles the functionality where "fleet adapter will compute the best possible itinerary given the current schedule and the other robot's negotiation submissions"

am actually asking about the classes that are searching for paths. i.e. using A* search to find an itinerary for a robot. I would think it should be in fleet_adapter package. the classes you listed from rmf_traffic package above are for negotiation after all itineraries from different fleet adapters are submitted right?

mxgrey commented 3 years ago

the classes you listed from rmf_traffic package above are for negotiation after all itineraries from different fleet adapters are submitted right?

Not quite. The classes I listed are the classes that are used to find a proposal (i.e. an itinerary) during a negotiation.

The A search is performed inside of the Planner class. The NegotiatingRouteValidator provides the A search with the constraints needed for negotiating (i.e. it tells the planner where the other robots intend to be). That constraint information is pulled from the Negotiation::Table::Viewer, which keeps track of one node within the overall negotiation. A single negotiation may involve up to N! "tables" where N is the number of participants involved in the negotiation. Each table requires one plan to be submitted by one robot, and the tables build on top of each other as those plans get submitted.

All of the planning and negotiation implementations are provided inside of the rmf_traffic package. The rmf_traffic_ros2 package just wraps the planning and negotiation utilities behind a ROS2 API, and the rmf_fleet_adapter package just wraps rmf_traffic and rmf_traffic_ros2 behind the simplest possible APIs for integrating a fleet. rmf_fleet_adapter also takes care of some multi-threading concerns (it creates many parallel threads so the rmf_traffic planning utilities can be used for many different tasks in parallel), but it doesn't implement any of the planning or negotiation itself.