Mutex groups, dynamic charging, and more (#310)

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>
Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>
Co-authored-by: Xiyu Oh <xiyu@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjoc@google.com>

rmf_charging_schedule/CHANGELOG.rst

@@ -0,0 +1,7 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rmf_charging_schedule
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.3.2 (XXXX-YY-ZZ)
+------------------
+* First release

rmf_charging_schedule/README.md

@@ -0,0 +1,39 @@
+A simple node that takes in a yaml file that describes a schedule for charger
+usage in an RMF scenario. The node will watch the clock and publish the
+appropriate commands to change the chargers of the robots.
+
+The format for the schedule looks like this:
+```
+"my_fleet_name":
+    "00:00": { "robot_1": "charger_A", "robot_2": "charger_B", "robot_3": "queue_A" }
+    "01:55": { "robot_1": "queue_B" }
+    "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" }
+```
+
+The time format is `"HH:MM"` where `HH` ranges from `00` to `23` and `MM` ranges
+from `00` to `59`. Note that quotes are important because otherwise the yaml
+format may confuse the meaning of the colon `:`.
+
+The schedule will cycle every 24 hours.
+
+For each timestamp, only robots that are explicitly mentioned will have their
+dedicated charger changed. **It is the responsibility of the schedule file author
+to make sure that two robots are never assigned the same charger at the same
+time.** Failing to ensure this may cause traffic and task management to misbehave.
+
+When run in simulation mode (`--ros-args --use-sim-time`), the time `00:00` in
+the schedule will correspond to `t=0.0` in simulation time.
+
+When run without sim time on, the hours and minutes will correspond to the local
+timezone of the machine that the node is run on. To choose a specific timezone
+instead of using the system's local timzeone, use the `--timezone` argument and
+provide the desired [TZ identifier](https://en.wikipedia.org/wiki/List_of_tz_database_time_zones)
+string.
+
+It is advisable that you always put a `00:00` entry that indicates **all** of
+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`.

rmf_charging_schedule/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rmf_charging_schedule</name>
+  <version>2.3.2</version>
+  <description>Node for a fixed 24-hour rotating charger usage schedule</description>
+  <maintainer email="mxgrey@intrinsic.ai">Grey</maintainer>
+  <license>Apache License 2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>rmf_fleet_msgs</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

rmf_charging_schedule/rmf_charging_schedule/main.py

@@ -0,0 +1,240 @@
+import sys
+import yaml
+import datetime
+import argparse
+from collections import OrderedDict
+import bisect
+from functools import total_ordering
+from zoneinfo import ZoneInfo
+
+from icecream import ic
+
+import rclpy
+from rclpy.node import Node, Publisher
+from rclpy.qos import (
+    QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy, QoSDurabilityPolicy
+)
+
+from rmf_fleet_msgs.msg import ChargingAssignment, ChargingAssignments
+
+@total_ordering
+class ScheduleTimePoint:
+    hour: int
+    minute: int
+
+    def __init__(self, hour: int, minute: int):
+        self.hour = hour
+        self.minute = minute
+
+    def parse(text: str):
+        segments = text.split(":")
+        assert len(segments) == 2, (
+            f'Time point text [{text}] does not have the correct HH:MM format'
+        )
+        hh = int(segments[0])
+        mm = int(segments[1])
+        assert 0 <= hh and hh < 24, (
+            f'Time point text [{text}] has an hour value which is outside the '
+            f'valid range of 0 -> 23.'
+        )
+        assert 0 <= mm and mm < 60, (
+            f'Time point text [{text}] has a minute value which is outside the '
+            f'valid range of 0 -> 59'
+        )
+        return ScheduleTimePoint(hh, mm)
+
+    def __eq__(self, other):
+        return self.hour == other.hour and self.minute == other.minute
+
+    def __lt__(self, other):
+        if self.hour < other.hour:
+            return True
+        elif self.hour > other.hour:
+            return False
+        return self.minute < other.minute
+
+    def __hash__(self):
+        return hash((self.hour, self.minute))
+
+
+class Assignment:
+    fleet: str
+    robot: str
+    charger: str
+
+    def __init__(self, fleet, robot, charger):
+        self.fleet = fleet
+        self.robot = robot
+        self.charger = charger
+
+
+def publish_assignments(publisher: Publisher, assignments: dict[dict[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
+            msg.assignments.append(assignment)
+
+        publisher.publish(msg)
+
+def update_assignments(
+    last_update_index: int | None,
+    next_update_index: int,
+    sorted_times: list,
+    schedule: dict,
+    assignments: dict,
+    publisher: Publisher,
+    node: Node,
+):
+    for key in sorted_times[last_update_index:next_update_index]:
+        changes: list[Assignment] = schedule[key]
+        for change in changes:
+            assignments.setdefault(change.fleet, {})[change.robot] = change.charger
+            node.get_logger().info(
+                f'Sending {change.fleet}/{change.robot} to {change.charger} at '
+                f'{key.hour:02d}:{key.minute:02d}'
+            )
+    publish_assignments(publisher, assignments)
+
+
+def simulation_time(node: Node) -> ScheduleTimePoint:
+    seconds, _ = node.get_clock().now().seconds_nanoseconds()
+    minutes: int = int(seconds/60)
+    hour: int = int((minutes/60) % 24)
+    minute = minutes % 60
+    return ScheduleTimePoint(hour, minute)
+
+
+def real_time(node: Node, timezone: ZoneInfo) -> ScheduleTimePoint:
+    nanoseconds = float(node.get_clock().now().nanoseconds)
+    seconds = nanoseconds / 1e9
+    dt = datetime.datetime.fromtimestamp(seconds, timezone)
+    return ScheduleTimePoint(dt.hour, dt.minute)
+
+
+def main(argv=sys.argv):
+    rclpy.init(args=argv)
+    node = Node("rmf_charging_schedule")
+    use_sim_time = node.get_parameter('use_sim_time').value
+
+    args_without_ros = rclpy.utilities.remove_ros_args(argv)
+
+    parser = argparse.ArgumentParser(
+        prog='rmf_charging_schedule',
+        description='Manage a fixed 24-hour charger schedule rotation',
+    )
+    parser.add_argument(
+        'schedule', type=str,
+        help=(
+            'A .yaml file representing the schedule. See README for the '
+            'expected format.'
+        )
+    )
+    parser.add_argument(
+        '-z', '--timezone', type=str, required=False,
+        help=(
+            'Timezone that the 24-hour rotation will be based on. If not '
+            'provided, the system\'s local timezone will be used.'
+        )
+    )
+    parser.add_argument(
+        '-t', '--test-time', action='store_true',
+        help=(
+            'Use this option to test the real time calculation by printing the '
+            'current HH:MM based on your settings. This may be used in '
+            'conjunction with the --timezone option and sim time. The node '
+            'will immediately quit after printing the time, so this will not '
+            'publish any assignment messages.'
+        )
+    )
+
+    args = parser.parse_args(args_without_ros[1:])
+    schedule_file = args.schedule
+
+    if args.timezone is not None:
+        timezone = ZoneInfo(args.timezone)
+    else:
+        timezone = None
+
+    if use_sim_time:
+        get_time = lambda: simulation_time(node)
+    else:
+        get_time = lambda: real_time(node, timezone)
+
+    if args.test_time:
+        t = get_time()
+        print(f'{t.hour:02d}:{t.minute:02d}')
+        return
+
+    with open(schedule_file, 'r') as f:
+        schedule_yaml = yaml.safe_load(f)
+
+    unsorted_schedule = {}
+    for fleet, change in schedule_yaml.items():
+        for time_text, assignments in change.items():
+            time = ScheduleTimePoint.parse(time_text)
+            entry: list[Assignment] = unsorted_schedule.get(time, list())
+            for robot, charger in assignments.items():
+                entry.append(Assignment(fleet, robot, charger))
+            unsorted_schedule[time] = entry
+
+    schedule = {}
+    sorted_times = []
+    for time in sorted(unsorted_schedule.keys()):
+        sorted_times.append(time)
+        schedule[time] = unsorted_schedule[time]
+
+    num_fleets = len(schedule_yaml.keys())
+    transient_qos = QoSProfile(
+        history=QoSHistoryPolicy.KEEP_LAST,
+        depth=2*num_fleets,
+        reliability=QoSReliabilityPolicy.RELIABLE,
+        durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+    )
+    publisher = node.create_publisher(
+        ChargingAssignments, 'charging_assignments', transient_qos
+    )
+
+    # fleet -> robot -> charger
+    assignments = {}
+    last_update_index = bisect.bisect_right(sorted_times, get_time())
+    update_assignments(
+        None, last_update_index,
+        sorted_times, schedule, assignments, publisher, node,
+    )
+
+    def update():
+        nonlocal last_update_index
+        nonlocal sorted_times
+        nonlocal schedule
+        nonlocal assignments
+        nonlocal publisher
+
+        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,
+            )
+            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,
+            )
+            last_update_index = next_update_index
+
+    node.create_timer(10.0, update)
+
+    rclpy.spin(node)
+
+if __name__ == '__main__':
+    main(sys.argv)

rmf_charging_schedule/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rmf_charging_schedule
+[install]
+install_scripts=$base/lib/rmf_charging_schedule

rmf_charging_schedule/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup, find_packages
+
+package_name = 'rmf_charging_schedule'
+
+setup(
+    name=package_name,
+    version='2.3.2',
+    packages=find_packages(),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Grey',
+    maintainer_email='mxgrey@intrinsic.ai',
+    description='A node that manages a fixed schedule for robot charger usage',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'charging_schedule=rmf_charging_schedule.main:main',
+        ],
+    },
+)

rmf_fleet_adapter/CMakeLists.txt

@@ -255,6 +255,25 @@ target_include_directories(lift_supervisor
 
 # -----------------------------------------------------------------------------
 
+add_executable(mutex_group_supervisor
+  src/mutex_group_supervisor/main.cpp
+)
+
+target_link_libraries(mutex_group_supervisor
+  PRIVATE
+    rmf_fleet_adapter
+    ${rclcpp_LIBARRIES}
+    ${rmf_fleet_msgs_LIBRARIES}
+)
+
+target_include_directories(mutex_group_supervisor
+  PRIVATE
+    ${rclcpp_INCLUDE_DIRS}
+    ${rmf_fleet_msgs_INCLUDE_DIRS}
+)
+
+# -----------------------------------------------------------------------------
+
 add_executable(experimental_lift_watchdog
   src/experimental_lift_watchdog/main.cpp
 )
@@ -502,6 +521,7 @@ install(
     mock_traffic_light
     full_control
     lift_supervisor
+    mutex_group_supervisor
     experimental_lift_watchdog
     door_supervisor
     robot_state_aggregator

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -68,6 +68,13 @@ const std::string InterruptRequestTopicName = "robot_interrupt_request";
 const std::string TaskApiRequests = "task_api_requests";
 const std::string TaskApiResponses = "task_api_responses";
 
+const std::string ChargingAssignmentsTopicName = "charging_assignments";
+
+const std::string MutexGroupRequestTopicName = "mutex_group_request";
+const std::string MutexGroupStatesTopicName = "mutex_group_states";
+
+const uint64_t Unclaimed = (uint64_t)(-1);
+
 } // namespace rmf_fleet_adapter
 
 #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP