From dfb656f16d5d3d73f76944fa1ec4b71cd01f5917 Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 10 Jun 2024 17:34:43 +0800 Subject: [PATCH] Fix charging status (#347) Signed-off-by: Michael X. Grey --- rmf_charging_schedule/README.md | 5 +++ .../rmf_charging_schedule/main.py | 31 ++++++++++++++----- .../src/rmf_fleet_adapter/TaskManager.cpp | 13 +++++++- .../rmf_fleet_adapter/agv/RobotContext.cpp | 13 ++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 10 +++++- .../agv/RobotUpdateHandle.cpp | 2 +- .../phases/WaitForCharge.cpp | 1 + .../phases/WaitForCharge.hpp | 1 + 8 files changed, 65 insertions(+), 11 deletions(-) diff --git a/rmf_charging_schedule/README.md b/rmf_charging_schedule/README.md index 3a14c818b..9e37973e0 100644 --- a/rmf_charging_schedule/README.md +++ b/rmf_charging_schedule/README.md @@ -10,6 +10,7 @@ The format for the schedule looks like this: "02:00": { "robot_3": "charger_A", "robot_1": "queue_A" } "03:55": { "robot_2": "queue_B" } "04:00": { "robot_1": "charger_B", "robot_2": "queue_A" } +parking: ["queue_A", "queue_B"] ``` The time format is `"HH:MM"` where `HH` ranges from `00` to `23` and `MM` ranges @@ -37,3 +38,7 @@ the intended charger assignments at midnight. When the node is launched, it will move through the schedule from the earliest entry up until the last relevant one and issue an initial charger assignment message based on what the assignments would have been if the schedule had run from `00:00`. + +If any of the waypoints are parking points instead of charging points, put them +into a list called `parking`. Note that this node does not support the existence +of a fleet with the name `parking`. diff --git a/rmf_charging_schedule/rmf_charging_schedule/main.py b/rmf_charging_schedule/rmf_charging_schedule/main.py index 9d90062c7..a4071e14b 100644 --- a/rmf_charging_schedule/rmf_charging_schedule/main.py +++ b/rmf_charging_schedule/rmf_charging_schedule/main.py @@ -68,7 +68,11 @@ def __init__(self, fleet, robot, charger): self.charger = charger -def publish_assignments(publisher: Publisher, assignments: dict[dict[str]]): +def publish_assignments( + publisher: Publisher, + assignments: dict[dict[str]], + parking: list[str] +): for fleet, robots in assignments.items(): msg = ChargingAssignments() msg.fleet_name = fleet @@ -76,9 +80,11 @@ def publish_assignments(publisher: Publisher, assignments: dict[dict[str]]): assignment = ChargingAssignment() assignment.robot_name = robot assignment.waypoint_name = charger - # The mode isn't actually used yet, so it doesn't matter what we set - # it to. - assignment.mode = ChargingAssignment.MODE_CHARGE + + if charger in parking: + assignment.mode = ChargingAssignment.MODE_WAIT + else: + assignment.mode = ChargingAssignment.MODE_CHARGE msg.assignments.append(assignment) publisher.publish(msg) @@ -89,6 +95,7 @@ def update_assignments( sorted_times: list, schedule: dict, assignments: dict, + parking: list[str], publisher: Publisher, node: Node, ): @@ -100,7 +107,7 @@ def update_assignments( f'Sending {change.fleet}/{change.robot} to {change.charger} at ' f'{key.hour:02d}:{key.minute:02d}' ) - publish_assignments(publisher, assignments) + publish_assignments(publisher, assignments, parking) def simulation_time(node: Node) -> ScheduleTimePoint: @@ -176,7 +183,14 @@ def main(argv=sys.argv): schedule_yaml = yaml.safe_load(f) unsorted_schedule = {} + parking = [] for fleet, change in schedule_yaml.items(): + if fleet == 'parking': + # We treat the parking entry as a special case that simply lists + # which waypoints are parking spots + parking = change + continue + for time_text, assignments in change.items(): time = ScheduleTimePoint.parse(time_text) entry: list[Assignment] = unsorted_schedule.get(time, list()) @@ -206,7 +220,7 @@ def main(argv=sys.argv): last_update_index = bisect.bisect_right(sorted_times, get_time()) update_assignments( None, last_update_index, - sorted_times, schedule, assignments, publisher, node, + sorted_times, schedule, assignments, parking, publisher, node, ) def update(): @@ -215,12 +229,13 @@ def update(): nonlocal schedule nonlocal assignments nonlocal publisher + nonlocal parking next_update_index = bisect.bisect_right(sorted_times, get_time()) if last_update_index < next_update_index: update_assignments( last_update_index, next_update_index, - sorted_times, schedule, assignments, publisher, node, + sorted_times, schedule, assignments, parking, publisher, node, ) last_update_index = next_update_index @@ -228,7 +243,7 @@ def update(): # The cycle must have restarted, e.g. passing midnight update_assignments( None, next_update_index, - sorted_times, schedule, assignments, publisher, node, + sorted_times, schedule, assignments, parking, publisher, node, ) last_update_index = next_update_index diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 947230885..38cca49bf 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -812,10 +812,21 @@ std::string TaskManager::robot_status() const if (_context->override_status().has_value()) return _context->override_status().value(); + if (_context->is_charging()) + { + if (_context->waiting_for_charger()) + { + return "idle"; + } + else + { + return "charging"; + } + } + if (!_active_task) return "idle"; - // TODO(MXG): Identify if the robot is charging and report that status here return "working"; } 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 6503edc2d..ec5ebb45d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -846,6 +846,18 @@ bool RobotContext::waiting_for_charger() const return _waiting_for_charger; } +//============================================================================== +std::shared_ptr RobotContext::be_charging() +{ + return _lock_charging; +} + +//============================================================================== +bool RobotContext::is_charging() const +{ + return _lock_charging.use_count() > 1; +} + //============================================================================== const rxcpp::observable& RobotContext::observe_battery_soc() const { @@ -1158,6 +1170,7 @@ RobotContext::RobotContext( _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), _charging_wp(state.dedicated_charging_waypoint().value()), + _lock_charging(std::make_shared(0)), _current_task_end_state(state), _current_task_id(std::nullopt), _task_planner(std::move(task_planner)), 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 a96be33e4..d4417b24f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -591,6 +591,13 @@ class RobotContext /// (false)? bool waiting_for_charger() const; + /// This function will indicate that the robot is currently charging for as + /// long as the return value is held onto. + std::shared_ptr be_charging(); + + /// Check if the robot is currently doing a battery charging task. + bool is_charging() const; + // Get a reference to the battery soc observer of this robot. const rxcpp::observable& observe_battery_soc() const; @@ -847,7 +854,8 @@ class RobotContext std::size_t _charging_wp; /// When the robot reaches its _charging_wp, is there to wait for a charger /// (true) or to actually charge (false)? - bool _waiting_for_charger; + bool _waiting_for_charger = false; + std::shared_ptr _lock_charging; rxcpp::subjects::subject _battery_soc_publisher; rxcpp::observable _battery_soc_obs; rmf_task::State _current_task_end_state; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index dba26ffab..54170b73a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -254,7 +254,7 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint( if (!self) return; - self->_set_charging(charger_wp, true); + self->_set_charging(charger_wp, false); RCLCPP_INFO( self->node()->get_logger(), "Charger waypoint for robot [%s] set to index [%ld]", diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp index 1b397e1cd..be9c81c67 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -119,6 +119,7 @@ WaitForCharge::Active::Active( } _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_CHARGING); + _lock_charging = _context->be_charging(); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp index b61389366..2c03adb62 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -76,6 +76,7 @@ class WaitForCharge rmf_traffic::Time _last_update_time; double _initial_battery_soc; double _expected_charging_rate; // % per hour + std::shared_ptr _lock_charging; };