Fix charging status (#347)

Signed-off-by: Michael X. Grey <[email protected]>

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`.

rmf_charging_schedule/rmf_charging_schedule/main.py

@@ -68,17 +68,23 @@ 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
         for robot, charger in robots.items():
             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,20 +229,21 @@ 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
 
         elif next_update_index < last_update_index:
             # 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
 

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";
 }
 

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<void> RobotContext::be_charging()
+{
+  return _lock_charging;
+}
+
+//==============================================================================
+bool RobotContext::is_charging() const
+{
+  return _lock_charging.use_count() > 1;
+}
+
 //==============================================================================
 const rxcpp::observable<double>& 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<int>(0)),
   _current_task_end_state(state),
   _current_task_id(std::nullopt),
   _task_planner(std::move(task_planner)),

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<void> 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<double>& 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<void> _lock_charging;
   rxcpp::subjects::subject<double> _battery_soc_publisher;
   rxcpp::observable<double> _battery_soc_obs;
   rmf_task::State _current_task_end_state;

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]",

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();
 }
 
 //==============================================================================

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<void> _lock_charging;
 
   };