Negotiation succeeded but it should have failed

[txlei]

Context is with two participants, one with full-control fleet adapter, p1, and another with StubbornNegotiator implementation, p2. p1 continues with its path even though p2 was in the way. p1's schedule in visualizer indicated red color. This occurs randomly though, i.e. sometimes, p1 did wait for p2's trajectory to finish and it moves.

Negotiation happens with the following logs but I cant make up what is actually happening after looking at the print negotiation status cpp file

...
[rmf_traffic_schedule-1] [1] Active negotiation: 1 4
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- [4:0]
[rmf_traffic_schedule-1]  -- 1:1 [4:0]
[rmf_traffic_schedule-1] 

the below happens if StubbornNegotiator is not constructed with p2

[rmf_traffic_schedule-1] [INFO] [1613295956.391043371] [rmf_traffic_schedule_node]: Refused negotiation [0]
[gzserver-13] [INFO] [1613295956.719494223] [slotcar_tinyRobot2]: tinyRobot2 reached waypoint 5/7
[gzserver-13] [INFO] [1613295956.720560445] [slotcar_tinyRobot2]: tinyRobot2 reached waypoint 6/7
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [1] Active negotiation: 1 3
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- [3:0]
[rmf_traffic_schedule-1]  -- 1:1 [3:0]

p2 implementation code

    node->writer->async_make_participant(
        std::move(description),
        [id, t = std::move(t), map_name](rmf_traffic::schedule::Participant p)
        {
            const int p_id = p.id();
            const std::map<int, Eigen::Vector3d>::iterator it = map.find(id);
            if (it != map.end()) {
                std::swap(map[p_id], it->second);
                map.erase(it);
            }

            node->participant[p_id] = std::move(p);
            RCLCPP_INFO(node->get_logger(), "Created participant with id: %d", p_id);
            print_detection_map();
            node->participant[p_id]->set({{map_name, std::move(t)}});
            
            auto negotiator = std::make_unique<rmf_traffic::schedule::StubbornNegotiator>(p);
            //node->_negotiation->register_negotiator(p_id, std::move(negotiator)); <--- causes node to shutdown 

            //auto mirror_future = rmf_traffic_ros2::schedule::make_mirror(
            //*node, rmf_traffic::schedule::query_all());

            //node->_mirror = mirror_future.get();
            //node->_negotiation = rmf_traffic_ros2::schedule::Negotiation(
            //*node, node->_mirror->snapshot_handle());

1. What could be actually happening with the negotiation between p1 and p2 now?
2. The stubborn negotiator is not successfully registered as I am not sure why it causes the current node to shutdown. Creating a handle does not matter I guess. So any other suggestion?
3. The stubborn negotiator do not know the current schedule. Using the respond function in StubbornNegotiator, how do I get the viewer from _mirror?
4. Do I need to have a rmf_traffic_ros2::schedule::Negotiation instance upon creating each participant with StubbornNegotiator?

[mxgrey]

I apologize for the esoteric printouts. We have plans to make more user-friendly introspection tools, but that hasn't reached the top of our priority list yet (though I expect it will soon).

[rmf_traffic_schedule-1] [1] Active negotiation: 1 4
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- [4:0]
[rmf_traffic_schedule-1]  -- 1:1 [4:0]
[rmf_traffic_schedule-1] 

This means that participant 1 has offered its ideal proposal, but participant 4 has not been responding to the negotiation at all. In general, [a:#] means the negotiation is waiting on participant a to respond.

auto negotiator = std::make_unique<rmf_traffic::schedule::StubbornNegotiator>(p);
node->_negotiation->register_negotiator(p_id, std::move(negotiator)); // <--- causes node to shutdown

To be pedantic, "node shutdown" typically refers to a graceful intentional exit. In this case, we won't get a shutdown, we will get a segmentation fault.

Earlier on this line:

node->participant[p_id] = std::move(p);

you used move semantics on p. After "moving" p, it no longer contains a valid value. Trying to use the variable p for anything will result in a segmentation fault. Unfortunately the C++ compiler is not smart enough to recognize this for you.

The easiest way to get this working would be to change it to the following:

auto negotiator = std::make_unique<rmf_traffic::schedule::StubbornNegotiator>(node->participant[p_id]);

I see one other error which will also cause your negotiations to get stuck. When you call

node->_negotiation->register_negotiator(p_id, std::move(negotiator));

you are discarding the handle that gets returned. As stated here in the documentation, this is a problem, because the negotiator will be immediately unregistered, so it will never participant in negotiating. To solve this, I would recommend something like this:

class MyCustomNode
{
  /* ... */
  struct ParticipantInfo
  {
    ParticipantInfo(rmf_traffic::schedule::Participant p, rmf_traffic_ros2::schedule::Negotiation& negotiation)
    : participant(std::move(p))
    {
      auto negotiator = std::make_unique<rmf_traffic::schedule::StubbornNegotiator>(participant);
      negotiation_license = negotiation.register_negotiator(participant.id(), std::move(negotiator));
    }

    rmf_traffic::schedule::Participant participant;
    std::shared_ptr<void> negotiation_license;
  };

  std::map<std::size_t, ParticipantInfo> participant_info;
};

Then to fill this in, you can put this in your callback:

node->participant[p_id] = ParticipantInfo(std::move(p), *node->_negotiation);
RCLCPP_INFO(node->get_logger(), "Created participant with id: %d", p_id);
print_detection_map();
node->participant[p_id]->set({{map_name, std::move(t)}});

Nothing should be needed after that.

The stubborn negotiator do not know the current schedule. Using the respond function in StubbornNegotiator, how do I get the viewer from _mirror?

You don't need to call the respond function of StubbornNegotiator. The rmf_traffic_ros2::schedule::Negotiation class will call that function automatically when needed.

Do I need to have a rmf_traffic_ros2::schedule::Negotiation instance upon creating each participant with StubbornNegotiator?

You should create exactly one rmf_traffic_ros2::schedule::Negotiation instance for your whole node. You should not allow any participants to be created until that Negotiation instance is created and available. I can see in some of your commented-out code that you were trying to make a mirror and then a negotiation handler within the schedule participant creation callback. That will not work. I'd suggest creating a make function for your node which ensures that all that construction is finished before anything else is allowed to be done with your node. You can use this as an example.

[txlei]

hi @mxgrey thank you for your explanation. however, with the short term fix on increasing the saturation limit, not all cases forces the robot to reroute.

examples with the same loop pantry -> station_1

obstacle introduced before sending loop request

successful reroute after second negotiation

another obstacle location, reroute never happens after multiple negotiations

there is another custom map I tested, with an almost square loop, where robot starts at a corner and either of the 2 possible paths is the approx the same distance. However, the robot chose the path with the obstacle even though there are no significant detour if it chose the other path.

All of its unsuccessful negotiation had been forfeited until robot passes the obstacle, negotiation is "resolved". Could you help me to understand the planner's behavior here?

//...
//forfeited negotiations 1-33

[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [33] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- 2:1 [1:0]
[rmf_traffic_schedule-1]  -- [1:0]
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [33] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- 2:1 [1:0]
[rmf_traffic_schedule-1]  -- 1:1 [2:0]
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [33] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- 2:1 [1:0]
[rmf_traffic_schedule-1]  -- {1:1}
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [33] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- {2:1}
[rmf_traffic_schedule-1]  -- {1:1}
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [33] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- <2:1>
[rmf_traffic_schedule-1]  -- {1:1}
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [33] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- <2:1>
[rmf_traffic_schedule-1]  -- <1:1>
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [INFO] [1613554882.665308685] [rmf_traffic_schedule_node]: Forfeited negotiation [33]
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [34] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- 2:1 [1:0]
[rmf_traffic_schedule-1]  -- [1:0]
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [34] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- 2:1 [1:0]
[rmf_traffic_schedule-1]  -- 1:1 [2:0]
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [34] Active negotiation: 1 2
[rmf_traffic_schedule-1]     Current negotiation state
[rmf_traffic_schedule-1]  -- 2:1 [1:0]
[rmf_traffic_schedule-1]  -- 1:1 >2:1<
[rmf_traffic_schedule-1] 
[rmf_traffic_schedule-1] [INFO] [1613554883.162414254] [rmf_traffic_schedule_node]: Resolved negotiation [34]: 1:1 2:1

I have seen tests with some successful reroutes but do not think that those problems were "easier" than those that failed. Trying to understand a little more before doing up a new world environment with one main loop with many "bridges" in between so that the planner has more routes to choose from.

[mxgrey]

I believe the second obstacle placement will work if you make the saturation limit even higher. It's a more "difficult" situation for the planner because the robot is able to get closer to the goal before it runs into the obstacle, and that causes the planner to try harder to go that way instead of taking the detour.

To explain why it's harder, the planner only knows about the presence of an obstacle by generating a trajectory for the robot and testing for collisions between that trajectory and anything in the schedule. The planner will try many many times to create trajectories through the obstacle, and with each attempt the A* search will branch off several nodes. All of those new nodes will make their own attempts to get through the obstacle, since they'll all be coming at it with different timings, so the planner thinks that maybe one of them could actually get through. This works okay most of the time for moving obstacles, because a moving obstacle will eventually either (1) eliminate most of the useless nodes by driving over them and/or (2) move out of the way so the search can advance. A permanent static obstacle is the worst case scenario for this algorithm, because it allows useless nodes to propagate rapidly. I do have a plan to make it smarter by improving the search heuristic to account for this kind of situation; I just haven't had time to implement it yet.

there is another custom map I tested, with an almost square loop, where robot starts at a corner and either of the 2 possible paths is the approx the same distance.

I'm very surprised that this would struggle. I'd be happy to debug if you're able to share that demo with me.

[txlei]

second obstacle placement will work if you make the saturation limit even higher.

yes, it worked on the second negotiation after first negotiation "fails" like the first example given. So what does forfeited negotiation means?

because the robot is able to get closer to the goal before it runs into the obstacle,

the robot hits the obstacle first then the goal. Do you mean, the obstacle is further away from the robot compared to the first example?

I'm very surprised that this would struggle.

the demo map is pushed to the map branch. I am wondering if the tinyrobot charger waypoint have anything got to do with it. since the nav graph might have some planner rules tagged to it.

[mxgrey]

yes, it worked on the second negotiation after first negotiation "fails" like the first example given. So what does forfeited negotiation means?

A forfeited negotiation means the planner failed to find any feasible plan for one or more of the participants to reach the goal within the resource constraints that the planner is given. The "resource constraints" for the planner are mostly just the saturation limit, which decides how many search nodes the planner is allowed to generate while trying to find a solution. If the saturation limit is too high, the computer could run out of RAM when trying to perform negotiations for multiple robots, and if that happens the process will crash. The heuristic improvements that I have in mind should allow arbitrarily difficult problems to be solved with relatively low saturation limits (if the improvements work as well as I'm imagining they will), but until those improvements are finished, we'll have to cope with the possibility of negotiations getting forfeited.

the robot hits the obstacle first then the goal. Do you mean, the obstacle is further away from the robot compared to the first example?

In the original obstacle placement (where it is very close to the pantry) the robot is never able to go down the west lane after pulling out of the pantry waypoint because the obstacle is right there blocking that whole lane off. For the second obstacle placement (when the obstacle is closer to the goal) the robot has the option of going down that west lane after pulling out of the pantry waypoint. At that point, there are three additional nodes (which are relatively close to the goal) that the planner can consider moving the robot to. For a human-level intelligence it's obvious to us that those nodes are not helpful for getting past the obstacle, but the planner doesn't have that intuition built into it (yet), so the planner will branch up and down those nodes many many times seeing if it can find some sequence of moving through those waypoints that will allow it to get past the obstacle. And increasing the branching factor can result in exponential increase in time and memory complexity, so even just a few more branching options can have a huge impact on performance. Of course as intelligent humans we know that branching up and down those nodes won't help, but the planner can't know that without the heuristic improvements that I have planned for it.

the demo map is pushed to the map branch.

Thanks, I'll take a look at it as soon as I have a chance.

[mxgrey]

I took a look at the map using the traffic editor, and I noticed a few of the nodes are disconnected from the lanes that they're supposed to be on. In the picture below I've dragged those nodes apart to make it clear and circled them in red:

I'm not sure if that's sufficient to explain the undesirable behavior that you described. If you can tell me specifically how to recreate the bad behavior, I'll be happy to run and diagnose it.