From 38a1266c8c858b4f0cd26c9e86df16c6f72a0438 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 1 Dec 2020 17:41:58 +0800 Subject: [PATCH 1/2] TaskManager initiates a ChargeBattery request when robot is idle and battery will fall below threshold --- .../src/rmf_fleet_adapter/TaskManager.cpp | 105 +++++++++++++++++- .../src/rmf_fleet_adapter/TaskManager.hpp | 11 +- .../agv/FleetUpdateHandle.cpp | 3 +- .../rmf_fleet_adapter/agv/RobotContext.cpp | 12 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 9 +- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 3 + rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 13 +++ 7 files changed, 148 insertions(+), 8 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 5bb3b5d77..2c5a88b20 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -24,6 +24,8 @@ #include +#include + #include "tasks/Clean.hpp" #include "tasks/ChargeBattery.hpp" #include "tasks/Delivery.hpp" @@ -52,7 +54,7 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) } }); - mgr->_timer = mgr->context()->node()->create_wall_timer( + mgr->_task_timer = mgr->context()->node()->create_wall_timer( std::chrono::seconds(1), [w = mgr->weak_from_this()]() { @@ -61,6 +63,16 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) mgr->_begin_next_task(); } }); + + mgr->_retreat_timer = mgr->context()->node()->create_wall_timer( + std::chrono::seconds(10), + [w = mgr->weak_from_this()]() + { + if (auto mgr = w.lock()) + { + mgr->retreat_to_charger(); + } + }); return mgr; } @@ -359,4 +371,95 @@ void TaskManager::clear_queue() _queue.clear(); } +//============================================================================== +void TaskManager::retreat_to_charger() +{ + if (_active_task || !_queue.empty()) + return; + + const auto task_planner = _context->task_planner(); + if (!task_planner) + return; + + const auto current_state = expected_finish_state(); + if (current_state.waypoint() == current_state.charging_waypoint()) + return; + + const double threshold_soc = _context->state_config().threshold_soc(); + const double retreat_threshold = 1.5 * threshold_soc; + const double current_battery_soc = _context->current_battery_soc(); + + const auto task_planner_config = task_planner->config(); + const auto estimate_cache = task_planner->estimate_cache(); + + double retreat_battery_drain = 0.0; + const auto endpoints = std::make_pair(current_state.waypoint(), + current_state.charging_waypoint()); + const auto& cache_result = estimate_cache->get(endpoints); + + if (cache_result) + { + retreat_battery_drain = cache_result->dsoc; + } + else + { + const rmf_traffic::agv::Planner::Goal retreat_goal{current_state.charging_waypoint()}; + const auto result_to_charger = task_planner_config->planner()->plan( + current_state.location(), retreat_goal); + + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - current_state.finish_time(); + + const double dSOC_motion = + task_planner_config->motion_sink()->compute_change_in_charge(trajectory); + const double dSOC_device = + task_planner_config->ambient_sink()->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + retreat_battery_drain = dSOC_motion + dSOC_device; + + // TODO(YV) Protect this call with a mutex + estimate_cache->set(endpoints, retreat_duration, + retreat_battery_drain); + } + + const double battery_soc_after_retreat = + current_battery_soc - retreat_battery_drain; + + if ((battery_soc_after_retreat < retreat_threshold) && + (battery_soc_after_retreat > threshold_soc)) + { + // Add a new charging task to the task queue + auto charging_request = rmf_task::requests::ChargeBattery::make( + task_planner_config->battery_system(), + task_planner_config->motion_sink(), + task_planner_config->ambient_sink(), + task_planner_config->planner(), + current_state.finish_time()); + + const auto finish = charging_request->estimate_finish( + current_state, + _context->state_config(), + estimate_cache); + + if (!finish) + return; + + rmf_task::agv::TaskPlanner::Assignment charging_assignment( + charging_request, + finish.value().finish_state(), + current_state.finish_time()); + + set_queue({charging_assignment}); + + RCLCPP_INFO( + _context->node()->get_logger(), + "Initiating automatic retreat to charger for robot [%s]", + _context->name().c_str()); + } +} + } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index b5c1bd224..7284684c7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -64,12 +64,16 @@ class TaskManager : public std::enable_shared_from_this /// Get the non-charging requests among pending tasks const std::vector requests() const; - // Callback for timer which begins next task if its deployment time has passed + /// Callback for task timer which begins next task if its deployment time has passed void _begin_next_task(); - // The state of the robot. + /// The state of the robot. State expected_finish_state() const; + /// Callback for the retreat timer. Appends a charging task to the task queue + /// when robot is idle and battery level drops below a retreat threshold. + void retreat_to_charger(); + private: TaskManager(agv::RobotContextPtr context); @@ -82,7 +86,8 @@ class TaskManager : public std::enable_shared_from_this rxcpp::subscription _emergency_sub; std::mutex _mutex; - rclcpp::TimerBase::SharedPtr _timer; + rclcpp::TimerBase::SharedPtr _task_timer; + rclcpp::TimerBase::SharedPtr _retreat_timer; void clear_queue(); }; 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 126f29a18..3c4bfc15d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -633,7 +633,8 @@ void FleetUpdateHandle::add_robot( fleet->_pimpl->worker, fleet->_pimpl->default_maximum_delay, state, - state_config + state_config, + fleet->_pimpl->task_planner }); // We schedule the following operations on the worker to make sure we do not diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 4b322710c..beed3ce6f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -253,6 +253,12 @@ const rxcpp::observable& RobotContext::observe_battery_soc() const return _battery_soc_obs; } +//============================================================================== +const std::shared_ptr& +RobotContext::task_planner() const +{ + return _task_planner; +} //============================================================================== void RobotContext::respond( @@ -284,7 +290,8 @@ RobotContext::RobotContext( const rxcpp::schedulers::worker& worker, rmf_utils::optional maximum_delay, rmf_task::agv::State state, - rmf_task::agv::StateConfig state_config) + rmf_task::agv::StateConfig state_config, + std::shared_ptr task_planner) : _command_handle(std::move(command_handle)), _location(std::move(_initial_location)), _itinerary(std::move(itinerary)), @@ -296,7 +303,8 @@ RobotContext::RobotContext( _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), _state(state), - _state_config(state_config) + _state_config(state_config), + _task_planner(std::move(task_planner)) { _profile = std::make_shared( _itinerary.description().profile()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 95d274960..cfb359ac5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -11,6 +11,7 @@ #include #include +#include #include @@ -134,6 +135,10 @@ class RobotContext // Get a reference to the battery soc observer of this robot. const rxcpp::observable& observe_battery_soc() const; + /// Get a mutable reference to the task planner for this robot + const std::shared_ptr& task_planner() const; + + private: friend class FleetUpdateHandle; friend class RobotUpdateHandle; @@ -148,7 +153,8 @@ class RobotContext const rxcpp::schedulers::worker& worker, rmf_utils::optional maximum_delay, rmf_task::agv::State state, - rmf_task::agv::StateConfig state_config); + rmf_task::agv::StateConfig state_config, + std::shared_ptr task_planner); std::weak_ptr _command_handle; std::vector _location; @@ -175,6 +181,7 @@ class RobotContext rxcpp::observable _battery_soc_obs; rmf_task::agv::State _state; rmf_task::agv::StateConfig _state_config; + std::shared_ptr _task_planner; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 4610426de..7496c6266 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -151,6 +151,9 @@ class TaskPlanner /// The configuration for the planner TaskPlanner(std::shared_ptr config); + /// Get a shared pointer to the configuration of this task planner + const std::shared_ptr config() const; + /// Get the greedy planner based assignments for a set of initial states and /// requests Assignments greedy_plan( diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 404d1eec2..bb846cea9 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -679,9 +679,11 @@ bool Filter::ignore(const Node& node) return !new_node; } +// ============================================================================ const rmf_traffic::Duration segmentation_threshold = rmf_traffic::time::from_seconds(1.0); +// ============================================================================ inline double compute_g_assignment(const TaskPlanner::Assignment& assignment) { if (std::dynamic_pointer_cast( @@ -696,6 +698,7 @@ inline double compute_g_assignment(const TaskPlanner::Assignment& assignment) } // anonymous namespace +// ============================================================================ class TaskPlanner::Implementation { public: @@ -1345,6 +1348,7 @@ class TaskPlanner::Implementation }; +// ============================================================================ TaskPlanner::TaskPlanner(std::shared_ptr config) : _pimpl(rmf_utils::make_impl( Implementation{ @@ -1355,6 +1359,7 @@ TaskPlanner::TaskPlanner(std::shared_ptr config) // Do nothing } +// ============================================================================ auto TaskPlanner::greedy_plan( rmf_traffic::Time time_now, std::vector initial_states, @@ -1370,6 +1375,7 @@ auto TaskPlanner::greedy_plan( true); } +// ============================================================================ auto TaskPlanner::optimal_plan( rmf_traffic::Time time_now, std::vector initial_states, @@ -1386,16 +1392,23 @@ auto TaskPlanner::optimal_plan( false); } +// ============================================================================ auto TaskPlanner::compute_cost(const Assignments& assignments) -> double { return _pimpl->compute_g(assignments); } +// ============================================================================ const std::shared_ptr TaskPlanner::estimate_cache() const { return _pimpl->estimate_cache; } +// ============================================================================ +const std::shared_ptr TaskPlanner::config() const +{ + return _pimpl->config; +} } // namespace agv } // namespace rmf_task From 1cb9dff5bad7d9d9b3be8732192c4ddd37ef644b Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 1 Dec 2020 17:55:38 +0800 Subject: [PATCH 2/2] Adjusted retreat threshold --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 2c5a88b20..f10a52d66 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -386,7 +386,7 @@ void TaskManager::retreat_to_charger() return; const double threshold_soc = _context->state_config().threshold_soc(); - const double retreat_threshold = 1.5 * threshold_soc; + const double retreat_threshold = 1.2 * threshold_soc; const double current_battery_soc = _context->current_battery_soc(); const auto task_planner_config = task_planner->config();