From 033fe3f4b97b464ebdcabae30578892bea77d1f0 Mon Sep 17 00:00:00 2001 From: carlychen Date: Mon, 1 Apr 2024 17:01:20 -0400 Subject: [PATCH] revert back the version of Task; update ParkRobotFactory so that it reads and uses robot's dedicated parking waypoint; added dedicated parking waypoing to robot state and method to load this parking waypoint --- rmf_task/include/rmf_task/State.hpp | 11 +++++++++ rmf_task/include/rmf_task/Task.hpp | 13 ++--------- rmf_task/src/rmf_task/State.cpp | 23 +++++++++++++++++++ rmf_task/src/rmf_task/Task.cpp | 19 ++++----------- .../src/rmf_task/requests/ChargeBattery.cpp | 3 +++ rmf_task/src/rmf_task/requests/Clean.cpp | 3 +++ rmf_task/src/rmf_task/requests/Delivery.cpp | 3 +++ rmf_task/src/rmf_task/requests/Loop.cpp | 3 +++ .../rmf_task/requests/ParkRobotFactory.cpp | 10 ++++---- 9 files changed, 58 insertions(+), 30 deletions(-) diff --git a/rmf_task/include/rmf_task/State.hpp b/rmf_task/include/rmf_task/State.hpp index 0770fd20..eedf2db9 100644 --- a/rmf_task/include/rmf_task/State.hpp +++ b/rmf_task/include/rmf_task/State.hpp @@ -57,6 +57,11 @@ class State : public CompositeData std::optional dedicated_charging_waypoint() const; State& dedicated_charging_waypoint(std::size_t new_charging_waypoint); + /// The dedicated parking spot for this robot + RMF_TASK_DEFINE_COMPONENT(std::size_t, DedicatedParkingPoint); + std::optional dedicated_parking_waypoint() const; + State& dedicated_parking_waypoint(std::size_t new_parking_waypoint); + /// The current battery state of charge of the robot. This value is between /// 0.0 and 1.0. RMF_TASK_DEFINE_COMPONENT(double, CurrentBatterySoC); @@ -78,6 +83,12 @@ class State : public CompositeData std::size_t charging_point, double battery_soc); + /// load the dedicated parking waypoint to the robot's State + /// + /// \param[in] parking_point + /// The robot's dedicated parking point. + State& load_parking_waypoint(std::size_t parking_point); + /// Load the plan start into the State. The location info will be split into /// CurrentWaypoint, CurrentOrientation, and CurrentTime data. State& load(const rmf_traffic::agv::Plan::Start& location); diff --git a/rmf_task/include/rmf_task/Task.hpp b/rmf_task/include/rmf_task/Task.hpp index 6c5f4197..489495d2 100644 --- a/rmf_task/include/rmf_task/Task.hpp +++ b/rmf_task/include/rmf_task/Task.hpp @@ -80,14 +80,11 @@ class Task::Booking /// \param[in] automatic /// Whether this booking was automatically generated /// - /// \param[in] labels - /// Labels to describe the purpose of the task dispatch request. Booking( std::string id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - bool automatic = false, - const std::vector& labels = {}); + bool automatic = false); /// Constructor /// @@ -110,16 +107,13 @@ class Task::Booking /// Whether this booking was automatically generated, default value as /// false. /// - /// \param[in] labels - /// Labels to describe the purpose of the task dispatch request. Booking( std::string id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, const std::string& requester, rmf_traffic::Time request_time, - bool automatic = false, - const std::vector& labels = {}); + bool automatic = false); /// The unique id for this booking. const std::string& id() const; @@ -141,9 +135,6 @@ class Task::Booking // Returns true if this booking was automatically generated. bool automatic() const; - /// Get the labels that describe the purpose of the task dispatch request. - std::vector labels() const; - class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_task/src/rmf_task/State.cpp b/rmf_task/src/rmf_task/State.cpp index 26bc6337..b57889cc 100644 --- a/rmf_task/src/rmf_task/State.cpp +++ b/rmf_task/src/rmf_task/State.cpp @@ -85,6 +85,22 @@ State& State::dedicated_charging_waypoint(std::size_t new_charging_waypoint) return *this; } +//============================================================================== +std::optional State::dedicated_parking_waypoint() const +{ + if (const auto* p = get()) + return p->value; + + return std::nullopt; +} + +//============================================================================== +State& State::dedicated_parking_waypoint(std::size_t new_parking_waypoint) +{ + with(new_parking_waypoint); + return *this; +} + //============================================================================== std::optional State::battery_soc() const { @@ -121,6 +137,13 @@ State& State::load_basic( return *this; } +//============================================================================== +State& State::load_parking_waypoint(std::size_t input_parking_point) +{ + dedicated_parking_waypoint(input_parking_point); + return *this; +} + //============================================================================== State& State::load(const rmf_traffic::agv::Plan::Start& location) { diff --git a/rmf_task/src/rmf_task/Task.cpp b/rmf_task/src/rmf_task/Task.cpp index ac9443a2..a1c5aa1d 100644 --- a/rmf_task/src/rmf_task/Task.cpp +++ b/rmf_task/src/rmf_task/Task.cpp @@ -31,7 +31,6 @@ class Task::Booking::Implementation std::optional requester; std::optional request_time; bool automatic; - std::vector labels; }; //============================================================================== @@ -39,8 +38,7 @@ Task::Booking::Booking( std::string id, rmf_traffic::Time earliest_start_time, ConstPriorityPtr priority, - bool automatic, - const std::vector& labels) + bool automatic) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(id), @@ -48,8 +46,7 @@ Task::Booking::Booking( std::move(priority), std::nullopt, std::nullopt, - automatic, - labels, + automatic })) { // Do nothing @@ -62,8 +59,7 @@ Task::Booking::Booking( ConstPriorityPtr priority, const std::string& requester, rmf_traffic::Time request_time, - bool automatic, - const std::vector& labels) + bool automatic) : _pimpl(rmf_utils::make_impl( Implementation{ std::move(id), @@ -71,8 +67,7 @@ Task::Booking::Booking( std::move(priority), requester, std::move(request_time), - automatic, - labels, + automatic })) { // Do nothing @@ -114,12 +109,6 @@ bool Task::Booking::automatic() const return _pimpl->automatic; } -//============================================================================== -std::vector Task::Booking::labels() const -{ - return _pimpl->labels; -} - //============================================================================== class Task::Tag::Implementation { diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 11a5c0d7..3a390373 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -109,6 +109,9 @@ ChargeBattery::Model::estimate_finish( std::move(final_plan_start), initial_state.dedicated_charging_waypoint().value(), initial_state.battery_soc().value()); + if (initial_state.dedicated_parking_waypoint()) { + state.load_parking_waypoint(initial_state.dedicated_parking_waypoint().value()); + } double battery_soc = initial_state.battery_soc().value(); rmf_traffic::Duration variant_duration(0); diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index b8707454..cedf1e9e 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -98,6 +98,9 @@ std::optional Clean::Model::estimate_finish( std::move(final_plan_start), initial_state.dedicated_charging_waypoint().value(), initial_state.battery_soc().value()); + if (initial_state.dedicated_parking_waypoint()) { + state.load_parking_waypoint(initial_state.dedicated_parking_waypoint().value()); + } rmf_traffic::Duration variant_duration(0); rmf_traffic::Duration end_duration(0); diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 9438669c..10ee086f 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -118,6 +118,9 @@ std::optional Delivery::Model::estimate_finish( std::move(final_plan_start), initial_state.dedicated_charging_waypoint().value(), initial_state.battery_soc().value()); + if (initial_state.dedicated_parking_waypoint()) { + state.load_parking_waypoint(initial_state.dedicated_parking_waypoint().value()); + } rmf_traffic::Duration variant_duration(0); diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index ea45126c..e7bab6f3 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -178,6 +178,9 @@ std::optional Loop::Model::estimate_finish( std::move(location), initial_state.dedicated_charging_waypoint().value(), battery_soc); + if (initial_state.dedicated_parking_waypoint()) { + finish_state.load_parking_waypoint(initial_state.dedicated_parking_waypoint().value()); + } // Check if robot can return to its charger if (drain_battery) diff --git a/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp index e8a66a5f..54b3c916 100644 --- a/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp +++ b/rmf_task/src/rmf_task/requests/ParkRobotFactory.cpp @@ -77,10 +77,12 @@ ParkRobotFactory::ParkRobotFactory( ConstRequestPtr ParkRobotFactory::make_request(const State& state) const { std::string id = "ParkRobot" + generate_uuid(); - const auto start_waypoint = state.waypoint().value(); - const auto finish_waypoint = _pimpl->parking_waypoint.has_value() ? - _pimpl->parking_waypoint.value() : - state.dedicated_charging_waypoint().value(); + const auto start_waypoint = state.waypoint().value_or(0); + + const std::size_t finish_waypoint = + _pimpl->parking_waypoint.has_value()? + _pimpl->parking_waypoint.value() : + state.dedicated_parking_waypoint().value_or(start_waypoint); if (_pimpl->requester.has_value() && _pimpl->time_now_cb) {