diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 2863e34d0..b3b0b6942 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -35,6 +35,8 @@ #include #include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -367,6 +369,13 @@ class FleetUpdateHandle : public std::enable_shared_from_this FleetUpdateHandle& set_update_listener( std::function listener); + /// Get the rclcpp::Node that this fleet update handle will be using for + /// communication. + std::shared_ptr node(); + + /// const-qualified node() + std::shared_ptr node() const; + // Do not allow moving FleetUpdateHandle(FleetUpdateHandle&&) = delete; FleetUpdateHandle& operator=(FleetUpdateHandle&&) = delete; diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 295dd456e..e979212fd 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -1234,10 +1234,25 @@ std::shared_ptr make_fleet( const std::string finishing_request_string = node->declare_parameter("finishing_request", "nothing"); rmf_task::ConstRequestFactoryPtr finishing_request = nullptr; + std::function get_time = + [n = std::weak_ptr(node)]() + { + const auto node = n.lock(); + if (!node) + { + const auto time_since_epoch = + std::chrono::system_clock::now().time_since_epoch(); + return rmf_traffic::Time(time_since_epoch); + } + return rmf_traffic_ros2::convert(node->now()); + }; + if (finishing_request_string == "charge") { finishing_request = - std::make_shared(); + std::make_shared( + std::string(node->get_name()), + std::move(get_time)); RCLCPP_INFO( node->get_logger(), "Fleet is configured to perform ChargeBattery as finishing request"); @@ -1245,7 +1260,9 @@ std::shared_ptr make_fleet( else if (finishing_request_string == "park") { finishing_request = - std::make_shared(); + std::make_shared( + std::string(node->get_name()), + std::move(get_time)); RCLCPP_INFO( node->get_logger(), "Fleet is configured to perform ParkRobot as finishing request"); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index b2459c7c0..a9e64d415 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -421,6 +421,17 @@ void copy_booking_data( booking_json["id"] = booking.id(); booking_json["unix_millis_earliest_start_time"] = to_millis(booking.earliest_start_time().time_since_epoch()).count(); + const auto requester = booking.requester(); + if (requester.has_value()) + { + booking_json["requester"] = requester.value(); + } + const auto request_time = booking.request_time(); + if (request_time.has_value()) + { + booking_json["unix_millis_request_time"] = + to_millis(request_time.value().time_since_epoch()).count(); + } // TODO(MXG): Add priority and labels } @@ -1576,7 +1587,11 @@ void TaskManager::retreat_to_charger() { // Add a new charging task to the task queue const auto charging_request = rmf_task::requests::ChargeBattery::make( - current_state.time().value()); + current_state.time().value(), + _context->requester_id(), + rmf_traffic_ros2::convert(_context->node()->now()), + nullptr, + true); const auto model = charging_request->description()->make_model( current_state.time().value(), parameters); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 803dc3243..eb40d6e2c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -238,6 +238,15 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( rmf_traffic::Time(std::chrono::milliseconds(t_it->get())); } + rmf_traffic::Time request_time = rmf_traffic_ros2::convert( + node->get_clock()->now()); + const auto r_it = request_msg.find("unix_millis_request_time"); + if (r_it != request_msg.end()) + { + request_time = + rmf_traffic::Time(std::chrono::milliseconds(r_it->get())); + } + // Note: make_low_priority() actually returns a nullptr. rmf_task::ConstPriorityPtr priority = rmf_task::BinaryPriorityScheme::make_low_priority(); @@ -279,11 +288,28 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( } } - const auto new_request = - std::make_shared( + std::optional requester = std::nullopt; + const auto i_it = request_msg.find("requester"); + if (i_it != request_msg.end()) + { + requester = i_it->get(); + } + + rmf_task::Task::ConstBookingPtr booking = requester.has_value() ? + std::make_shared( + task_id, + earliest_start_time, + priority, + requester.value(), + request_time, + false) : + std::make_shared( task_id, earliest_start_time, priority, + false); + const auto new_request = std::make_shared( + std::move(booking), deserialized_task.description); return new_request; @@ -1995,6 +2021,18 @@ FleetUpdateHandle& FleetUpdateHandle::set_update_listener( return *this; } +//============================================================================== +std::shared_ptr FleetUpdateHandle::node() +{ + return _pimpl->node; +} + +//============================================================================== +std::shared_ptr FleetUpdateHandle::node() const +{ + return _pimpl->node; +} + //============================================================================== bool FleetUpdateHandle::set_task_planner_params( std::shared_ptr battery_system, @@ -2044,7 +2082,7 @@ bool FleetUpdateHandle::set_task_planner_params( // automatic retreat. Hence, we also update them whenever the // task planner here is updated. self->_pimpl->task_planner = std::make_shared( - std::move(task_config), std::move(options)); + self->_pimpl->name, std::move(task_config), std::move(options)); for (const auto& t : self->_pimpl->task_managers) t.first->task_planner(self->_pimpl->task_planner); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index 71e3a0c13..d64f29ef2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -53,7 +53,17 @@ rmf_task::Task::ActivePtr EmergencyPullover::start( std::make_shared()), {}); const auto desc = builder.build("Emergency Pullover", ""); - const rmf_task::Request request(task_id, context->now(), nullptr, desc, true); + + const auto time_now = context->now(); + rmf_task::Task::ConstBookingPtr booking = + std::make_shared( + task_id, + time_now, + nullptr, + context->requester_id(), + time_now, + true); + const rmf_task::Request request(std::move(booking), desc); return activator.activate( context->make_get_state(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp index 85d180baf..079e921ed 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp @@ -70,7 +70,17 @@ rmf_task::Task::ActivePtr ResponsiveWait::start( ResponsiveWait::Description::make_indefinite(waiting_point)), {}); const auto desc = builder.build("Responsive Wait", ""); - const rmf_task::Request request(task_id, context->now(), nullptr, desc, true); + + const auto time_now = context->now(); + rmf_task::Task::ConstBookingPtr booking = + std::make_shared( + task_id, + time_now, + nullptr, + context->requester_id(), + time_now, + true); + const rmf_task::Request request(std::move(booking), desc); return context->task_activator()->activate( context->make_get_state(), diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 80968a7cc..f0aace9f9 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -4,6 +4,7 @@ #include #include #include "pybind11_json/pybind11_json.hpp" +#include #include #include "rmf_traffic_ros2/Time.hpp" @@ -333,21 +334,38 @@ PYBIND11_MODULE(rmf_adapter, m) { bool account_for_battery_drain, const std::string& finishing_request_string = "nothing") { + std::function time_now = nullptr; + std::optional planner_id = std::nullopt; + const auto node = self.node(); + if (node) + { + time_now = [n = node->weak_from_this()]() + { + const auto node = n.lock(); + if (!node) + { + const auto time_since_epoch = + std::chrono::system_clock::now().time_since_epoch(); + return rmf_traffic::Time(time_since_epoch); + } + return rmf_traffic_ros2::convert(node->now()); + }; + planner_id = node->get_name(); + } + // Supported finishing_request_string: [charge, park, nothing] - rmf_task::ConstRequestFactoryPtr finishing_request; + rmf_task::ConstRequestFactoryPtr finishing_request = nullptr; if (finishing_request_string == "charge") { - finishing_request = - std::make_shared(); + finishing_request = planner_id.has_value() && time_now ? + std::make_shared(planner_id.value(), std::move(time_now)) : + std::make_shared(); } else if (finishing_request_string == "park") { - finishing_request = - std::make_shared(); - } - else - { - finishing_request = nullptr; + finishing_request = planner_id.has_value() && time_now ? + std::make_shared(planner_id.value(), std::move(time_now)) : + std::make_shared(); } return self.set_task_planner_params( diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index a9269ef7b..07c973927 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -800,8 +800,10 @@ class Dispatcher::Implementation static const std::vector copy_fields = { "unix_millis_earliest_start_time", + "unix_millis_request_time", "priority", - "labels" + "labels", + "requester", }; for (const auto& field : copy_fields)