From ed06ff7166b138fcdcf3861b49c881073d73d459 Mon Sep 17 00:00:00 2001 From: kjchee Date: Fri, 17 Apr 2026 15:36:35 +0800 Subject: [PATCH] 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