From ed06ff7166b138fcdcf3861b49c881073d73d459 Mon Sep 17 00:00:00 2001 From: kjchee Date: Fri, 17 Apr 2026 15:36:35 +0800 Subject: [PATCH 1/4] Add event class for GoToZone feature Signed-off-by: kjchee --- .../rmf_task_sequence/events/GoToZone.hpp | 61 +++++++ .../src/rmf_task_sequence/events/GoToZone.cpp | 163 ++++++++++++++++++ 2 files changed, 224 insertions(+) create mode 100644 rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp create mode 100644 rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp new file mode 100644 index 0000000..b0336f8 --- /dev/null +++ b/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp @@ -0,0 +1,61 @@ +#ifndef RMF_TASK_SEQUENCE__EVENTS__GOTOZONE_HPP +#define RMF_TASK_SEQUENCE__EVENTS__GOTOZONE_HPP + +#include + +#include + +namespace rmf_task_sequence { +namespace events { + +//============================================================================== +class GoToZone +{ +public: + class Description; + using DescriptionPtr = std::shared_ptr; + using ConstDescriptionPtr = std::shared_ptr; +}; + +//============================================================================== +class GoToZone::Description : public Event::Description +{ +public: + struct Modifiers + { + std::string group_hint; + std::optional orientation_hint; + std::vector preferred_waypoints; + }; + + /// Make a GoToZone description using a zone name and optional modifiers. + static DescriptionPtr make( + std::string zone_name, + std::optional modifiers = std::nullopt); + + /// Get the name of the zone for this description. + const std::string& zone_name() const; + + /// Get the modifiers for this description. + const std::optional& modifiers() const; + + // Documentation inherited + Activity::ConstModelPtr make_model( + State invariant_initial_state, + const Parameters& parameters) const final; + + // Documentation inherited + Header generate_header( + const State& initial_state, + const Parameters& parameters) const final; + + class Implementation; +private: + Description(); + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace events +} // namespace rmf_task_sequence + +#endif // RMF_TASK_SEQUENCE__EVENTS__GOTOZONE_HPP diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp new file mode 100644 index 0000000..b46d661 --- /dev/null +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp @@ -0,0 +1,163 @@ +#include +#include + +#include "utils.hpp" + +namespace rmf_task_sequence { +namespace events { + +namespace { +// Wraps a GoToPlace Activity::Model and rejects robots whose current +// waypoint is inside the target zone. Prevents robots already in the +// zone from winning bids for zone tasks targeting the same zone. +class ZoneGuardModel : public Activity::Model +{ +public: + ZoneGuardModel( + Activity::ConstModelPtr inner, + std::vector zone_waypoints) + : _inner(std::move(inner)), + _zone_waypoints(std::move(zone_waypoints)) + { + // Do nothing + } + + std::optional estimate_finish( + rmf_task::State initial_state, + rmf_traffic::Time earliest_arrival_time, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const override + { + if (initial_state.waypoint().has_value()) + { + const auto wp = *initial_state.waypoint(); + for (const auto& zone_wp : _zone_waypoints) + { + if (wp == zone_wp) + return std::nullopt; + } + } + return _inner->estimate_finish( + std::move(initial_state), earliest_arrival_time, + constraints, travel_estimator); + } + + rmf_traffic::Duration invariant_duration() const override + { + return _inner->invariant_duration(); + } + + rmf_task::State invariant_finish_state() const override + { + return _inner->invariant_finish_state(); + } + +private: + Activity::ConstModelPtr _inner; + std::vector _zone_waypoints; +}; +} // anonymous namespace + +//============================================================================== +class GoToZone::Description::Implementation +{ +public: + std::string zone_name; + std::optional modifiers; +}; + +//============================================================================== +auto GoToZone::Description::make( + std::string zone_name, + std::optional modifiers) -> DescriptionPtr +{ + auto desc = std::shared_ptr(new Description); + desc->_pimpl = rmf_utils::make_impl( + Implementation{std::move(zone_name), std::move(modifiers)}); + + return desc; +} + +//============================================================================== +Activity::ConstModelPtr GoToZone::Description::make_model( + State invariant_initial_state, + const Parameters& parameters) const +{ + const auto& graph = parameters.planner()->get_configuration().graph(); + const auto zone_props = graph.find_known_zone(_pimpl->zone_name); + if (!zone_props) + return nullptr; + + std::vector goals; + std::vector zone_wp_indices; + for (const auto& iv : zone_props->internal_vertices()) + { + const auto* wp = graph.find_waypoint(iv.name()); + if (!wp) + continue; + goals.emplace_back(wp->index()); + zone_wp_indices.push_back(wp->index()); + } + + if (goals.empty()) + return nullptr; + + const auto place_desc = + GoToPlace::Description::make_for_one_of(std::move(goals)); + auto inner_model = place_desc->make_model( + std::move(invariant_initial_state), parameters); + if (!inner_model) + return nullptr; + + // Wrap in ZoneGuardModel so that robots already at a zone vertex + // get filtered out during bidding (estimate_finish returns nullopt) + Activity::ConstModelPtr guard = std::make_shared( + std::move(inner_model), std::move(zone_wp_indices)); + return guard; +} + +//============================================================================== +Header GoToZone::Description::generate_header( + const State& initial_state, + const Parameters& parameters) const +{ + const std::string& fail_header = "[GoToZone::Description::generate_header]"; + const auto& graph = parameters.planner()->get_configuration().graph(); + const auto start_wp_opt = initial_state.waypoint(); + if (!start_wp_opt) + utils::fail(fail_header, "Initial state is missing a waypoint"); + + const auto start_name = + rmf_task::standard_waypoint_name(graph, *start_wp_opt); + + const auto model = make_model(initial_state, parameters); + const auto duration = model ? model->invariant_duration() : + rmf_traffic::Duration(0); + + return Header( + "Go to zone " + _pimpl->zone_name, + "Moving robot from " + start_name + " to zone " + _pimpl->zone_name, + duration); +} + +//============================================================================== +const std::string& GoToZone::Description::zone_name() const +{ + return _pimpl->zone_name; +} + +//============================================================================== +const std::optional& +GoToZone::Description::modifiers() const +{ + return _pimpl->modifiers; +} + +//============================================================================== +GoToZone::Description::Description() +{ + // Do nothing +} + +} // namespace events +} // namespace rmf_task_sequence \ No newline at end of file From ad409360d2e31a50e6985690ad241ce3d60ecc0c Mon Sep 17 00:00:00 2001 From: kjchee Date: Mon, 14 Sep 2026 10:57:30 +0800 Subject: [PATCH 2/4] Make GoToZone Modifiers a pimpl class Signed-off-by: kjchee --- .../rmf_task_sequence/events/GoToZone.hpp | 31 ++++++++-- .../src/rmf_task_sequence/events/GoToZone.cpp | 60 +++++++++++++++++++ 2 files changed, 87 insertions(+), 4 deletions(-) diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp index b0336f8..b8da9ac 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp @@ -21,11 +21,34 @@ class GoToZone class GoToZone::Description : public Event::Description { public: - struct Modifiers + /// Hints for which waypoint in the zone to assign. + class Modifiers { - std::string group_hint; - std::optional orientation_hint; - std::vector preferred_waypoints; + public: + /// Construct with no hints set. + Modifiers(); + + /// Get the waypoint group to prefer. Empty means no preference. + const std::string& group_hint() const; + + /// Set the waypoint group to prefer. + Modifiers& set_group_hint(std::string hint); + + /// Get the orientation to prefer at the waypoint, in radians. + std::optional orientation_hint() const; + + /// Set the orientation to prefer at the waypoint, in radians. + Modifiers& set_orientation_hint(std::optional hint); + + /// Get the waypoints to prefer, in order. + const std::vector& preferred_waypoints() const; + + /// Set the waypoints to prefer, in order. + Modifiers& set_preferred_waypoints(std::vector waypoints); + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; }; /// Make a GoToZone description using a zone name and optional modifiers. diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp index b46d661..1036434 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp @@ -159,5 +159,65 @@ GoToZone::Description::Description() // Do nothing } +//============================================================================== +class GoToZone::Description::Modifiers::Implementation +{ +public: + std::string group_hint; + std::optional orientation_hint; + std::vector preferred_waypoints; +}; + +//============================================================================== +GoToZone::Description::Modifiers::Modifiers() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +const std::string& GoToZone::Description::Modifiers::group_hint() const +{ + return _pimpl->group_hint; +} + +//============================================================================== +auto GoToZone::Description::Modifiers::set_group_hint(std::string hint) +-> Modifiers& +{ + _pimpl->group_hint = std::move(hint); + return *this; +} + +//============================================================================== +std::optional +GoToZone::Description::Modifiers::orientation_hint() const +{ + return _pimpl->orientation_hint; +} + +//============================================================================== +auto GoToZone::Description::Modifiers::set_orientation_hint( + std::optional hint) -> Modifiers& +{ + _pimpl->orientation_hint = hint; + return *this; +} + +//============================================================================== +const std::vector& +GoToZone::Description::Modifiers::preferred_waypoints() const +{ + return _pimpl->preferred_waypoints; +} + +//============================================================================== +auto GoToZone::Description::Modifiers::set_preferred_waypoints( + std::vector waypoints) -> Modifiers& +{ + _pimpl->preferred_waypoints = std::move(waypoints); + return *this; +} + } // namespace events } // namespace rmf_task_sequence \ No newline at end of file From 81613a2ff1b21763b0fbc9cbb8b03fbd69bb8d8e Mon Sep 17 00:00:00 2001 From: kjchee Date: Mon, 14 Sep 2026 15:03:54 +0800 Subject: [PATCH 3/4] Replace ZoneGuardModel with a nearest-vertex ZoneModel Signed-off-by: kjchee --- .../src/rmf_task_sequence/events/GoToZone.cpp | 75 +++++++++---------- 1 file changed, 36 insertions(+), 39 deletions(-) diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp index 1036434..9d749a5 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp @@ -7,19 +7,21 @@ namespace rmf_task_sequence { namespace events { namespace { -// Wraps a GoToPlace Activity::Model and rejects robots whose current -// waypoint is inside the target zone. Prevents robots already in the -// zone from winning bids for zone tasks targeting the same zone. -class ZoneGuardModel : public Activity::Model +//============================================================================== +// One GoToPlace model per zone vertex, costed against the nearest. +class ZoneModel : public Activity::Model { public: - ZoneGuardModel( - Activity::ConstModelPtr inner, - std::vector zone_waypoints) - : _inner(std::move(inner)), - _zone_waypoints(std::move(zone_waypoints)) + ZoneModel( + std::vector candidates) + : _candidates(std::move(candidates)), + _nearest(_candidates.front()) { - // Do nothing + for (const auto& candidate : _candidates) + { + if (candidate->invariant_duration() < _nearest->invariant_duration()) + _nearest = candidate; + } } std::optional estimate_finish( @@ -28,33 +30,37 @@ class ZoneGuardModel : public Activity::Model const rmf_task::Constraints& constraints, const rmf_task::TravelEstimator& travel_estimator) const override { - if (initial_state.waypoint().has_value()) + std::optional best; + for (const auto& candidate : _candidates) { - const auto wp = *initial_state.waypoint(); - for (const auto& zone_wp : _zone_waypoints) + auto estimate = candidate->estimate_finish( + initial_state, earliest_arrival_time, constraints, travel_estimator); + if (!estimate.has_value()) + continue; + + if (!best.has_value() + || *estimate->finish_state().time() < *best->finish_state().time()) { - if (wp == zone_wp) - return std::nullopt; + best = std::move(estimate); } } - return _inner->estimate_finish( - std::move(initial_state), earliest_arrival_time, - constraints, travel_estimator); + + return best; } rmf_traffic::Duration invariant_duration() const override { - return _inner->invariant_duration(); + return _nearest->invariant_duration(); } rmf_task::State invariant_finish_state() const override { - return _inner->invariant_finish_state(); + return _nearest->invariant_finish_state(); } private: - Activity::ConstModelPtr _inner; - std::vector _zone_waypoints; + std::vector _candidates; + Activity::ConstModelPtr _nearest; }; } // anonymous namespace @@ -88,32 +94,23 @@ Activity::ConstModelPtr GoToZone::Description::make_model( if (!zone_props) return nullptr; - std::vector goals; - std::vector zone_wp_indices; + std::vector candidates; for (const auto& iv : zone_props->internal_vertices()) { const auto* wp = graph.find_waypoint(iv.name()); if (!wp) continue; - goals.emplace_back(wp->index()); - zone_wp_indices.push_back(wp->index()); - } - if (goals.empty()) - return nullptr; + auto model = GoToPlace::Description::make(wp->index()) + ->make_model(invariant_initial_state, parameters); + if (model) + candidates.push_back(std::move(model)); + } - const auto place_desc = - GoToPlace::Description::make_for_one_of(std::move(goals)); - auto inner_model = place_desc->make_model( - std::move(invariant_initial_state), parameters); - if (!inner_model) + if (candidates.empty()) return nullptr; - // Wrap in ZoneGuardModel so that robots already at a zone vertex - // get filtered out during bidding (estimate_finish returns nullopt) - Activity::ConstModelPtr guard = std::make_shared( - std::move(inner_model), std::move(zone_wp_indices)); - return guard; + return std::make_shared(std::move(candidates)); } //============================================================================== From b088c5e2dd51075c2cc4a9c04cd0932dea57d70c Mon Sep 17 00:00:00 2001 From: kjchee Date: Sat, 10 Oct 2026 08:29:05 +0800 Subject: [PATCH 4/4] Take an ordered list of hints in GoToZone Description Signed-off-by: kjchee --- .../rmf_task_sequence/events/GoToZone.hpp | 45 +++++++++------- .../src/rmf_task_sequence/events/GoToZone.cpp | 52 +++++++++---------- 2 files changed, 49 insertions(+), 48 deletions(-) diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp index b8da9ac..11bc71a 100644 --- a/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp +++ b/rmf_task_sequence/include/rmf_task_sequence/events/GoToZone.hpp @@ -21,46 +21,51 @@ class GoToZone class GoToZone::Description : public Event::Description { public: - /// Hints for which waypoint in the zone to assign. - class Modifiers + /// One preference for the zone assignment: where to park, and facing + /// which way. Name a group, or a waypoint, or neither to accept any + /// waypoint in the zone. + class Hint { public: - /// Construct with no hints set. - Modifiers(); + /// Construct with nothing named, which accepts any waypoint at any + /// orientation. + Hint(); - /// Get the waypoint group to prefer. Empty means no preference. - const std::string& group_hint() const; + /// Get the group to prefer. Empty if none. + const std::string& group() const; - /// Set the waypoint group to prefer. - Modifiers& set_group_hint(std::string hint); + /// Set the group to prefer. + Hint& set_group(std::string group); - /// Get the orientation to prefer at the waypoint, in radians. - std::optional orientation_hint() const; + /// Get the waypoint to prefer. Empty if none. + const std::string& waypoint() const; - /// Set the orientation to prefer at the waypoint, in radians. - Modifiers& set_orientation_hint(std::optional hint); + /// Set the waypoint to prefer. + Hint& set_waypoint(std::string waypoint); - /// Get the waypoints to prefer, in order. - const std::vector& preferred_waypoints() const; + /// Get the orientations the robot may park at, in radians. Empty means + /// any. + const std::vector& orientations() const; - /// Set the waypoints to prefer, in order. - Modifiers& set_preferred_waypoints(std::vector waypoints); + /// Set the orientations the robot may park at, in radians. + Hint& set_orientations(std::vector orientations); class Implementation; private: rmf_utils::impl_ptr _pimpl; }; - /// Make a GoToZone description using a zone name and optional modifiers. + /// Make a GoToZone description using a zone name and ordered hints, most + /// preferred first. static DescriptionPtr make( std::string zone_name, - std::optional modifiers = std::nullopt); + std::vector hints = {}); /// Get the name of the zone for this description. const std::string& zone_name() const; - /// Get the modifiers for this description. - const std::optional& modifiers() const; + /// Get the hints for this description, most preferred first. + const std::vector& hints() const; // Documentation inherited Activity::ConstModelPtr make_model( diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp index 9d749a5..76be82f 100644 --- a/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp +++ b/rmf_task_sequence/src/rmf_task_sequence/events/GoToZone.cpp @@ -69,17 +69,17 @@ class GoToZone::Description::Implementation { public: std::string zone_name; - std::optional modifiers; + std::vector hints; }; //============================================================================== auto GoToZone::Description::make( std::string zone_name, - std::optional modifiers) -> DescriptionPtr + std::vector hints) -> DescriptionPtr { auto desc = std::shared_ptr(new Description); desc->_pimpl = rmf_utils::make_impl( - Implementation{std::move(zone_name), std::move(modifiers)}); + Implementation{std::move(zone_name), std::move(hints)}); return desc; } @@ -144,10 +144,10 @@ const std::string& GoToZone::Description::zone_name() const } //============================================================================== -const std::optional& -GoToZone::Description::modifiers() const +const std::vector& +GoToZone::Description::hints() const { - return _pimpl->modifiers; + return _pimpl->hints; } //============================================================================== @@ -157,62 +157,58 @@ GoToZone::Description::Description() } //============================================================================== -class GoToZone::Description::Modifiers::Implementation +class GoToZone::Description::Hint::Implementation { public: - std::string group_hint; - std::optional orientation_hint; - std::vector preferred_waypoints; + std::string group; + std::string waypoint; + std::vector orientations; }; //============================================================================== -GoToZone::Description::Modifiers::Modifiers() +GoToZone::Description::Hint::Hint() : _pimpl(rmf_utils::make_impl()) { // Do nothing } //============================================================================== -const std::string& GoToZone::Description::Modifiers::group_hint() const +const std::string& GoToZone::Description::Hint::group() const { - return _pimpl->group_hint; + return _pimpl->group; } //============================================================================== -auto GoToZone::Description::Modifiers::set_group_hint(std::string hint) --> Modifiers& +auto GoToZone::Description::Hint::set_group(std::string group) -> Hint& { - _pimpl->group_hint = std::move(hint); + _pimpl->group = std::move(group); return *this; } //============================================================================== -std::optional -GoToZone::Description::Modifiers::orientation_hint() const +const std::string& GoToZone::Description::Hint::waypoint() const { - return _pimpl->orientation_hint; + return _pimpl->waypoint; } //============================================================================== -auto GoToZone::Description::Modifiers::set_orientation_hint( - std::optional hint) -> Modifiers& +auto GoToZone::Description::Hint::set_waypoint(std::string waypoint) -> Hint& { - _pimpl->orientation_hint = hint; + _pimpl->waypoint = std::move(waypoint); return *this; } //============================================================================== -const std::vector& -GoToZone::Description::Modifiers::preferred_waypoints() const +const std::vector& GoToZone::Description::Hint::orientations() const { - return _pimpl->preferred_waypoints; + return _pimpl->orientations; } //============================================================================== -auto GoToZone::Description::Modifiers::set_preferred_waypoints( - std::vector waypoints) -> Modifiers& +auto GoToZone::Description::Hint::set_orientations( + std::vector orientations) -> Hint& { - _pimpl->preferred_waypoints = std::move(waypoints); + _pimpl->orientations = std::move(orientations); return *this; }