EasyFullControl

[xiyuoh]

This PR continues from #189 to implement an EasyFullControl API for users to create their own fleet adapters more easily.

This PR can be tested with open-rmf/rmf_demos#158. You can run any full control demo world with easy_fleet:=1:

ros2 launch rmf_demos_gz_classic office.launch.xml easy_fleet:=1

Further to-do:

• Transformer between RMF and robot coordinates
• Update with fixes from Async behavior fixes rmf_demos#157
• Python bindings

[mxgrey]

I think this promises to be a huge usability improvement over the existing full control API. There's really only one aspect of it that I think should be reworked.. although unfortunately it's a big one.

I believe the update callbacks are sort of "inverted" from how they should be. As an example

  using NavigationRequest =
    std::function<GoalCompletedCallback(
        const std::string& map_name,
        const Eigen::Vector3d location,
        RobotUpdateHandlePtr robot_handle)>;

This implies that when a navigation request is issued to the user side, we expect them to return a

using GoalCompletedCallback = std::function<GoalStatus()>;

to us, which we will then poll to figure out when the request has been completed. I see this as problematic for two reasons:

• We are placing a burden on the user to guard these callbacks with mutexes. On our end, we have no way of protecting the data that's being accessed when we trigger this callback. That means if the user does not carefully guard their callback implementations and their application data with mutexes, then we are at risk of data races. In the worst case, this could even lead to undefined behavior. I think this is too heavy of a burden to put on users.
• We will be doing a lot of needless polling. It might not be terribly noticeable at the scales we're currently testing on, but it's hard to anticipate how we might want our systems to scale in the future.

So I believe we need to invert this relationship, maybe like this:

class RobotUpdateHandle::ActionExecution
{
public:
    /// Update position. For navigation requests this will also update the remaining time
    void update_position(const std::string& map_name, Eigen::Vector3d position);

    /// Update the amount of time remaining for this action.
    /// This does not need to be used for navigation requests.
    void update_remaining_time(rmf_traffic::Duration remaining_time_estimate);

    /// Set task status to underway and optionally log a message (info tier)
    void underway(std::optional<std::string> text);

    /// Set task status to error and optionally log a message (error tier)
    void error(std::optional<std::string> text);

    /// Set the task status to delayed and optionally log a message
    /// (warning tier)
    void delayed(std::optional<std::string> text);

    /// Set the task status to blocked and optionally log a message
    /// (warning tier)
    void blocked(std::optional<std::string> text);

    /// Trigger this when the action is successfully finished.
    /// No other functions in this ActionExecution instance will
    /// be usable after this.
    void finished();

    /// Returns false if the Action has been killed or cancelled
    bool okay() const;
   
     /// Get the RobotUpdateHandle
    RobotUpdateHandlePtr handle() const;

    class Implementation;
  private:
    ActionExecution();
    rmf_utils::impl_ptr<Implementation> _pimpl;
};

using NavigationRequest = 
  std::function<void(
    const std::string& map_name, 
    const Eigen::Vector3d location, 
    RobotUpdateHandle::ActionExecution execution)>;

Note that I changed a few things about the ActionExecution class to better suit using it for navigation. I think it provides us with a good foundation to reuse for the EasyFullControl API.

The user would be responsible for triggering the update functions in ActionExecution as information arrives. We could have an internal timeout where we re-issue a navigation request if we don't receive an update after an unreasonable amount of time.

I think the same principles should be applied to DockRequest.

I apologize that I didn't have time to make these changes myself before going on PTO, but I don't think I'd feel comfortable with moving this API out of experimental until these changes are made.

[Yadunund]

Sorry had a error in my previous comment

    void override_schedule(const std::string& map_name, const rmf_traffic::Trajectory& trajectory);

[xiyuoh]

afef64c

[Yadunund]

We don't have to pass in a bool arg for this function. Users can simply call this function to request for a replan. It can return a bool depending on if the replanning succeeded/failed.
bool replan();

[xiyuoh]

afef64c

[Yadunund]

We should move this function outside this Transform class but in the same header/namespace and have it accept a transform object (Same as how it was in EasyFullControl.hpp).

[xiyuoh]

33dc617

[mxgrey]

I see that an update_position() method is added to ActionExecution to update the robot's map name and position, which is not necessary as we already have a GetStateCallback retrieving the robot's RobotState on every update.

Sorry for not making this part clear: I think we should get rid of GetStateCallback and RobotState entirely. I don't think it's a good idea for us to poll for this data when

1. Some of the fields won't change very often or ever (e.g. name and charger_name)
2. The fields inside of RobotState are largely independent, so it doesn't make sense to bundle them
3. The user is forced to manage mutexes in their implementation of the GetStateCallback to make sure they can collect and return all this information safely when reads/writes are happening across multiple threads

I'm going to experiment a bit with how to straighten out these details. Ideally I'd like us to have an API where the user (system integrator) never needs to touch a mutex, unlike the current situation where we've found that every function in the command handle of rmf_demos_fleet_adapter needs to be guarded by a mutex to avoid risk of serious race condition bugs.

[Yadunund]

I see that an update_position() method is added to ActionExecution to update the robot's map name and position, which is not necessary as we already have a GetStateCallback retrieving the robot's RobotState on every update.

Sorry for not making this part clear: I think we should get rid of GetStateCallback and RobotState entirely. I don't think it's a good idea for us to poll for this data when

1. Some of the fields won't change very often or ever (e.g. name and charger_name)
2. The fields inside of RobotState are largely independent, so it doesn't make sense to bundle them
3. The user is forced to manage mutexes in their implementation of the GetStateCallback to make sure they can collect and return all this information safely when reads/writes are happening across multiple threads

I'm going to experiment a bit with how to straighten out these details. Ideally I'd like us to have an API where the user (system integrator) never needs to touch a mutex, unlike the current situation where we've found that every function in the command handle of rmf_demos_fleet_adapter needs to be guarded by a mutex to avoid risk of serious race condition bugs.

That's a lot better 👍🏼

Out of date