Handling RobotContext position/map methods more gracefully

Open-RMF Open-RMF General
1

Any ideas on how to make RobotContext::position and RobotContext::map() more graceful instead of throwing std::runtime_error as this will crash the fleet adapter.

2

Have you encountered a situation where those get thrown?

The API is supposed to be designed so that you are forced to provide a location value when initializing the robot, and then all the APIs for updating the location should ensure that some kind of position value remains. At the very least the last known usable value should be kept in _lost.

If there’s a sequence of API calls that can lead to both _location and _lost being empty, then that would be a bug in the API, and we’ll need to know how to reproduce that in order to fix it.

3

yes. we encountered that today for the first time..
is it possible to pass empty location_ here ?

4

I think the root problem is this line, which is ill-conceived. We should never do set_lost(std::nullopt) because that leaves us entirely without a fallback location for the robot.

Instead the first thing we should do in RobotContext::set_location is determine what the fallback location should be based on

  1. the current this->_location value
  2. the incoming location_ value
  3. the current this->_lost value

Then if we end up at that set_lost(_) line, we should pass in that fallback location instead of passing in std::nullopt.

5

Actually what is the intention of this block

  ...
  _location = std::move(location_);
  filter_closed_lanes();

  if (_location.empty())
  {
    if (debug_positions)
    {
      std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST" <<
        std::endl;
    }
    set_lost(std::nullopt);
    return;
  }
  else if (_lost)
  {
     ...
  }
6 
  _location = std::move(location_);
  filter_closed_lanes();

This takes the locations that were passed into the function and then filters out any that would attach the robot to a closed lane. We filter out closed lanes because the integration code that passes in location_ doesn’t necessary know which lanes are closed, and we want to prevent the planner from relying on closed lanes as part of the plan that they generate.

However after filtering we could end up with an empty _location. Or the user could’ve passed in an empty location_ from the start. When this happens, we want the robot to be set into the “lost” mode, which will add an issue notification into the robot state that operator dashboards could escalate to human operators. That’s what set_lost is supposed to do.

So if (_location.empty()) will call set_lost to indicate that we’re in a lost state, but the intention when we become lost is to also preserve the last known valid location so that we have something to pass back when something inside rmf_fleet_adapter asks for position or map. Somehow this set_lost(std::nullopt) slipped through a review, and puts the RobotContext in a state that it’s not supposed to be in, where it is both lost and has no last known good location information.

What we should do instead is look through all possible sources of a valid location (this->_location, location_, and this->_lost) and preserve at least one usable (position, map) pair from any one of those sources. If we always handle this correctly, there should always be a valid (position, map) pair available within the RobotContext.

1 Like