Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

EasyFullControl #235

Merged
merged 46 commits into from
Aug 18, 2023
Merged

EasyFullControl #235

merged 46 commits into from
Aug 18, 2023

Conversation

xiyuoh
Copy link
Member

@xiyuoh xiyuoh commented Sep 13, 2022

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:

xiyuoh and others added 14 commits March 25, 2022 18:08
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
@xiyuoh xiyuoh mentioned this pull request Sep 13, 2022
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Yadunund and others added 3 commits October 7, 2022 16:20
* Update API

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Changes to impl

Signed-off-by: Yadunund <yadunund@gmail.com>

* Implemented more functions

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* More impl changes

Signed-off-by: Yadunund <yadunund@gmail.com>

* Delete the right file

Signed-off-by: Yadunund <yadunund@gmail.com>

* Fix build

Signed-off-by: Yadunund <yadunund@gmail.com>

* Style

Signed-off-by: Yadunund <yadunund@gmail.com>

* Bindings WIP

Signed-off-by: Yadunund <yadunund@gmail.com>

* Return correct values

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Fix dangling reference

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Update bindings for RobotState and Configuration

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Undo constructor arg

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Fix bug

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Fix bug

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Do not stop for new paths

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Update charger only when state changes

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Make config optional

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Default value of remaining time

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Update bindings to new API

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Spin adapter to declare nodes

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Update bindings

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* run -> wait

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Reliability fixes

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Decrease lane merge distance

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Further reduce lane distance

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Add documentation

Signed-off-by: Yadunund <yadunund@gmail.com>

* Correct typo (#245)

Spotted during ROSCon open-rmf workshop.

Signed-off-by: Yadunund <yadunund@openrobotics.org>
Signed-off-by: Yadunund <yadunund@gmail.com>
Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
Co-authored-by: Luca Della Vedova <luca@openrobotics.org>
Co-authored-by: Andrew Ring <andrew@andrewring.dev>
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Michael X. Grey <grey@openrobotics.org>
Co-authored-by: Michael X. Grey <grey@openrobotics.org>
@mxgrey
Copy link
Contributor

mxgrey commented Dec 9, 2022

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.

/// trajectory. This should not be used while tasks with automatic schedule
/// updating are running, or else the traffic schedule will have jumbled up
/// information, which can be disruptive to the overall traffic management.
void override_schedule(std::string map_name, rmf_traffic::Trajectory trajectory);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry had a error in my previous comment

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

Copy link
Member Author

@xiyuoh xiyuoh May 31, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@@ -152,6 +152,16 @@ class RobotUpdateHandle
/// (warning tier)
void blocked(std::optional<std::string> text);

/// Trigger this when you require a replan for your navigation or
/// docking request
void replan(bool request_replan);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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();

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

/// Helper function to transform between RMF and robot coordinate systems.
/// Depending on the Transformation defined, this function can be used to
/// transform robot coordinate system to RMF's coordinate system or vice versa.
const Eigen::Vector3d transform(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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).

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Signed-off-by: Xi Yu Oh <xiyuoh@intrinsic.ai>
Signed-off-by: Xi Yu Oh <xiyuoh@intrinsic.ai>
@mxgrey
Copy link
Contributor

mxgrey commented Jun 7, 2023

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
Copy link
Member

Yadunund commented Jun 8, 2023

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 👍🏼

mxgrey and others added 2 commits August 14, 2023 14:53
Signed-off-by: Michael X. Grey <grey@openrobotics.org>
Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>
Co-authored-by: Xiyu <xiyu@openrobotics.org>
Yadunund
Yadunund previously approved these changes Aug 14, 2023
Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>
@xiyuoh xiyuoh dismissed stale reviews from Yadunund and mxgrey via 2cff1a6 August 17, 2023 06:56
@xiyuoh xiyuoh requested a review from mxgrey August 17, 2023 06:57
…to issues in the underlying middleware implementation

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
mxgrey
mxgrey previously approved these changes Aug 17, 2023
@mxgrey mxgrey merged commit 75a0656 into main Aug 18, 2023
4 checks passed
@mxgrey mxgrey deleted the feature/easy_full_control branch August 18, 2023 05:17
Yadunund added a commit that referenced this pull request Sep 18, 2023
Signed-off-by: Xi Yu Oh <xiyu@openrobotics.org>
Signed-off-by: Yadunund <yadunund@openrobotics.org>
Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
Signed-off-by: Michael X. Grey <grey@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyuoh@intrinsic.ai>
Co-authored-by: Yadunund <yadunund@openrobotics.org>
Co-authored-by: Luca Della Vedova <luca@openrobotics.org>
Co-authored-by: Michael X. Grey <grey@openrobotics.org>
xiyuoh added a commit that referenced this pull request Sep 19, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants