diff --git a/_images/docs/wait_until_task_A.png b/_images/docs/wait_until_task_A.png
new file mode 100644
index 0000000..2ed668d
Binary files /dev/null and b/_images/docs/wait_until_task_A.png differ
diff --git a/_images/docs/wait_until_task_B.png b/_images/docs/wait_until_task_B.png
new file mode 100644
index 0000000..bbd2dfe
Binary files /dev/null and b/_images/docs/wait_until_task_B.png differ
diff --git a/_images/docs/wait_until_task_C.png b/_images/docs/wait_until_task_C.png
new file mode 100644
index 0000000..6fa395a
Binary files /dev/null and b/_images/docs/wait_until_task_C.png differ
diff --git a/_images/docs/wait_until_task_D.png b/_images/docs/wait_until_task_D.png
new file mode 100644
index 0000000..02698ec
Binary files /dev/null and b/_images/docs/wait_until_task_D.png differ
diff --git a/configs/mir_config.yaml b/configs/mir_config.yaml
index 0e5609d..d841b3a 100644
--- a/configs/mir_config.yaml
+++ b/configs/mir_config.yaml
@@ -111,3 +111,31 @@ plugins:
# Examples of additional fields for cart detection module
io_name: "MiR internal IOs"
latch_io: 2
+ rmf_wait_until:
+ module: "fleet_adapter_mir_actions.rmf_wait_until"
+ actions: ["wait_until"]
+ timeout: 60 # Optional, in seconds. Timeout if there is no move-off signal received, after which robot will move off regardless. Default to 60 seconds.
+ update_gap: 30 # Optional, in seconds. The fleet adapter will log an update using this interval. Default to 30 seconds.
+ signals: # Configure various move off signal types. If this field is not provided in the fleet config or task description, the robot will wait the full duration of the default timeout.
+ # Currently three signal types are supported: [mission, plc, custom].
+ sample_mission_1: # Only a unique name
+ signal_type: "mission" # Specifies the actual type
+ mission_name: "some_mission_name" # If this mission is found on the robot, it will be queued when the action starts, and the robot will move off when the mission is completed.
+ retry_count: 10 # Optional field for signal type "mission". The fleet adapter will re-attempt queueing the mission for this number of tries.
+ resubmit_on_abort: False # Optional field for signal type "mission". Set to True to resubmit mission if the mission gets aborted by the robot. Default to False.
+ sample_mission_2:
+ signal_type: "mission" # Specifies the actual type
+ mission_name: "some_other_mission_name"
+ sample_plc_1:
+ signal_type: "plc" # Specifies the actual type
+ register: 20 # Fill in with PLC register number. Robot moves off when this PLC register returns True
+ sample_plc_2:
+ signal_type: "plc" # Specifies the actual type
+ register: 21
+ custom_1:
+ signal_type: "custom" # Specifies the actual type
+ module: "fleet_adapter_mir_actions.rmf_move_off_on_alert" # Fill in with path to custom module written. Robot moves off when module provides the user-defined move off signal.
+ custom_2:
+ signal_type: "custom" # Specifies the actual type
+ module: "fleet_adapter_mir_actions.rmf_move_off_on_something_else"
+ default_signal: "custom_1" # Specifies a default signal type out of those listed above. If the task description does not provide any signal config, the fleet adapter will use this default signal type.
diff --git a/docs/mir_actions.md b/docs/mir_actions.md
index 15dd518..51d0f73 100644
--- a/docs/mir_actions.md
+++ b/docs/mir_actions.md
@@ -5,6 +5,7 @@ This section lists and elaborates on the MiR actions provided out-of-the-box in
## Available Action Plugins
* [rmf_cart_delivery](#rmf_cart_delivery)
+* [rmf_wait_until](#rmf_wait_until)
## rmf_cart_delivery
@@ -54,3 +55,108 @@ ros2 run fleet_adapter_mir_tasks dispatch_delivery -g go_to_waypoint -p pickup_l
- `-p`: Name of the pickup lot. This name should be identical to the shelf position configured on the MiR.
- `-d`: Name of the dropoff lot. This name should be identical to the robot or shelf position configured on the MiR.
- `-c`: Optional cart identifier for the fleet adapter to assess whether the cart is correct for pickup.
+
+
+## rmf_wait_until
+
+### Overview
+
+The `rmf_wait_until` plugin introduces the `wait_until` action. It allows users to command a robot to wait at a specified location until it receives a move off signal or until a configured timeout. The robot would then be free to move on to carry out the remainder of its task, or complete the task and proceed to its idle behavior. During this waiting period, the user may command the robot to perform missions or any customized behavior with user-defined move off signals to trigger completion.
+
+This action can come in handy for various use cases, for example:
+- The robot has to perform a delivery where it travels between pick up and drop off locations, and wait at each location for an external device to load or unload items on itself.
+- The robot is performing a MiR mission at a specific location, and is only ready to move off when the mission is completed.
+
+Here is the workflow of a multi-stop task, demonstrating how the `wait_until` action can be used:
+1. Users submit a task with a list of waypoints, RMF will send the robot to the first waypoint
+2. The robot will stop at the waypoint and do nothing until it receives a move off signal or until a configured timeout.
+3. Repeat Steps 1. and 2. until the robot has travelled to all the waypoints in the task.
+
+This plugin currently supports three move off signal types:
+1. Move off when a MiR mission completes.
+ - Users may create/select a MiR mission on the robot and provide the mission name in the fleet config or task description. This mission will be submitted to the robot when the waiting action begins. The waiting action will end when the robot receives the move off signal, which in this case happens when the robot completes the mission.
+2. Move off when a PLC register returns `True`.
+ - When the waiting action begins, the fleet adapter will monitor the state of the PLC register specified in the fleet config or task description. When the register returns a non-zero integer or `True`, the waiting action will end and the robot will move on to its next waypoint or task. Numeric strings convertible to integers are also accepted, e.g. "1", "5".
+3. Move off on a user-defined signal.
+ - Users can customize their own signal type by implementing a `MoveOff` module.
+
+The plugin also supports task-specific move off signals, such that users can trigger different move off signals in each task. These move off signals are either pre-defined in the plugin config, or configured in the task description (with the exception of `custom` signals). It is up to the user to ensure that these signal types are valid and compatible with the MiR.
+
+You may refer to the Examples section below to get a clearer idea of how to customize signal types with plugin config and task descriptions.
+
+### Setup
+
+Users can configure multiple move off signals for their robot fleet in the fleet's plugin config, as long as they are supported by the WaitUntil action. If signal types are not configured in the plugin config nor the task description, the robot will not have a move off signal set up, and simply wait for the full duration of the timeout during the action.
+
+Steps for setup:
+
+1. Fill in the appropriate fields under the `rmf_wait_until` plugin in the fleet config, with reference to the example provided in `mir_config.yaml`. It allows users to customize the behavior of their robots during the waiting action and the type of signal to trigger move off.
+ - `timeout`: Optional, the default timeout of the waiting action. At timeout, the robot will move off even if it did not receive the move off signal. If not specified, the timeout will default to 60 seconds. This value can be overridden by the task description.
+ - `update_gap`: Optional, the update interval for logging purposes. If not specified, the update gap will default to 30 seconds. This value can be overridden by the task description.
+ - `signals`: List the signals to be preconfigured for the fleet. We currently support `mission`, `plc` and `custom` signal types. These signals can be triggered when specified in the task description.
+ - Provide a unique name for each of your signal, with a mandatory field `signal_type`. Refer to `mir_config.yaml` for examples for each supported type.
+ - The following elaborates on each supported type and their respective config:
+ - For the `mission` signal type, users will have to provide the following fields:
+ - `mission_name` is required for the robot to trigger the relevant MiR mission when the waiting action starts. This is a compulsory field and may be overridden by the task description.
+ - `retry_count` is an integer and an optional field, used to configure the number of times the fleet adapter should re-attempt posting a mission at the start of the waiting action. If the mission cannot be successfully queued on the robot beyond the number of retries, the task will be cancelled. This value can be overridden by the task description.
+ - `resubmit_on_abort` is a boolean and an optional field, used to configure the action behavior in cases where the MiR mission cannot be successfully completed and gets aborted by the robot. When set to `True`, the fleet adapter will submit the same MiR mission to the robot if the previous attempt has been aborted. The move off signal will come only when the mission has been successfully completed. If set to `False`, the fleet adapter will treat aborted missions as completed and end the waiting action. This value can be overridden by the task description.
+ - For the `plc` signal type, users will need to provide the relevant PLC register number. This register number should be an integer and available on the MiR. This value can be overridden by the task description.
+ - For the `custom` signal type, users will need to fill in the path to their custom `MoveOff` module that is implemented from the `BaseMoveOff` abstract class provided.
+ - The `default_signal` field is optional but encouraged to be added. It should point to any one of the configured signals. If no signal is provided in the task description, the fleet adapter will select the default signal from the plugin config as the move off trigger. If neither is provided, the robot will simply wait for the full duration of the timeout since it does not have a move off signal configured.
+2. [Optional] Create your own `MoveOff` plugin.
+ - You are encouraged to use the `BaseMoveOff` class in `rmf_move_off.py` as a base for your own module implementation. The class methods will be used by the `WaitUntil` Mir Action.
+ - In the plugin config, update the `custom` field to point to your own written module.
+
+
+### Usage
+
+To submit a multi-stop waiting task, you may use the `dispatch_multistop` task script found in the `fleet_adapter_mir_tasks` package:
+```bash
+# Trigger a task with a configured signal custom_2
+ros2 run fleet_adapter_mir_tasks dispatch_multistop -g waypoint_1 waypoint_2 waypoint_3 -s custom_2
+
+# Trigger a task with a new mission signal and a timeout of 120 seconds
+ros2 run fleet_adapter_mir_tasks dispatch_multistop -g waypoint_1 waypoint_2 waypoint_3 -t 120 -s mission -m some_mission_name
+
+# Trigger a task with a new PLC signal
+ros2 run fleet_adapter_mir_tasks dispatch_multistop -g waypoint_1 waypoint_2 waypoint_3 -s plc -p 30
+
+# Trigger the default signal configured in plugin config
+ros2 run fleet_adapter_mir_tasks dispatch_multistop -g waypoint_1 waypoint_2 waypoint_3
+```
+- `-g`: Takes in the waypoints the robots should travel to for each waiting action.
+- `-t`: Optional timeout of the action in seconds. Default to 60 seconds.
+- `-u`: Optional update gap of the action in seconds. Default to 30 seconds.
+- `-s`: Signal/signal type for this `wait_until` action, currently supported options are `mission`, `plc`, and any of the pre-configured signals.
+- `-m`: Further specifies the mission name for signal type `mission`.
+- `-r`: A boolean determining whether to resubmit missions if they are aborted by the robot. Used for signal type `mission`.
+- `-rc`: An integer indicating the number of times to reattempt queueing a mission. Used for signal type `mission`.
+- `-p`: Further specifies the PLC register number for signal type `plc`.
+
+Do note that this task involves using the same move off signal for every waypoint the robot travels to.
+
+### Examples
+
+
+Example 1: Select a configured signal
+
+
+
+
+
+Example 2: Configuring a signal type not found in plugin config
+
+
+
+
+
+Example 3: Use the pre-configured default signal
+
+
+
+
+
+Example 4: Let robot simply wait during the action without any move off signal configured
+
+
+
diff --git a/fleet_adapter_mir/setup.py b/fleet_adapter_mir/setup.py
index 8ccc66f..592ecaf 100644
--- a/fleet_adapter_mir/setup.py
+++ b/fleet_adapter_mir/setup.py
@@ -21,6 +21,7 @@
'console_scripts': [
'fleet_adapter_mir=fleet_adapter_mir.fleet_adapter_mir:main',
'rmf_cart_delivery=fleet_adapter_mir.rmf_cart_delivery:CartDelivery',
+ 'rmf_wait_until=fleet_adapter_mir.rmf_wait_until:WaitUntil',
],
},
)
diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_move_off.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_move_off.py
new file mode 100644
index 0000000..7f5133e
--- /dev/null
+++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_move_off.py
@@ -0,0 +1,40 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from abc import ABC, abstractmethod
+from fleet_adapter_mir.robot_adapter_mir import ActionContext
+
+
+class BaseMoveOff(ABC):
+ def __init__(self, context: ActionContext):
+ self.context = context
+
+ '''
+ This method is called when the robot reaches the waiting waypoint and
+ begins waiting. Use this callback to trigger any process during the
+ waiting period.
+ '''
+ @abstractmethod
+ def begin_waiting(self, description: dict):
+ # To be implemented
+ ...
+
+ '''
+ This method checks if the robot is ready to move off.
+ Returns True if ready, else False.
+ '''
+ @abstractmethod
+ def is_move_off_ready(self) -> bool:
+ # To be implemented
+ ...
diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_move_off_on_alert.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_move_off_on_alert.py
new file mode 100644
index 0000000..4dc75bc
--- /dev/null
+++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_move_off_on_alert.py
@@ -0,0 +1,104 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import datetime
+from threading import Lock
+
+from rclpy.qos import qos_profile_system_default
+from rclpy.qos import QoSDurabilityPolicy as Durability
+from rclpy.qos import QoSHistoryPolicy as History
+from rclpy.qos import QoSProfile
+from rclpy.qos import QoSReliabilityPolicy as Reliability
+from rmf_task_msgs.msg import Alert
+from rmf_task_msgs.msg import AlertResponse
+from rmf_task_msgs.msg import AlertParameter
+
+from fleet_adapter_mir.robot_adapter_mir import ActionContext
+
+
+class MoveOff(BaseMoveOff):
+ def __init__(self, context: ActionContext):
+ MoveOff.__init__(self, context)
+
+ '''
+ This example demonstrates how we can notify the robot to move off when
+ it receives an Alert via ROS 2.
+ '''
+ self.mutex = Lock()
+ self.alert = None
+ self.ready = False
+
+ # Create alert related publisher and subscribers
+ transient_qos = QoSProfile(
+ history=History.KEEP_LAST,
+ depth=1,
+ reliability=Reliability.RELIABLE,
+ durability=Durability.TRANSIENT_LOCAL,
+ )
+ self.alert_response_sub = self.context.node.create_subscription(
+ AlertResponse,
+ 'alert_response',
+ self.alert_response_cb,
+ qos_profile=transient_qos
+ )
+ self.alert_pub = self.context.node.create_publisher(
+ Alert,
+ 'alert',
+ qos_profile=transient_qos
+ )
+
+ def begin_waiting(self, description):
+ # Publish an alert to signal that the robot has begun waiting
+ msg = Alert()
+ msg.id = datetime.datetime.now().strftime(
+ "alert-%Y-%m-%d-%H-%M-%S")
+ msg.title = f'Robot [{self.context.name}] has begun waiting.'
+ msg.tier = Alert.TIER_INFO
+ msg.responses_available = ['ready']
+ msg.task_id = self.context.update_handle.more().current_task_id()
+ self.alert_pub.publish(msg)
+ self.context.node.get_logger().info(
+ f'Robot [{self.context.name}] published alert [{msg.id}] to '
+ f'signal that it has started waiting.'
+ )
+ self.alert = msg
+
+ def is_move_off_ready(self):
+ with self.mutex:
+ if self.ready:
+ # This `ready` variable is created everytime the robot begins a
+ # wait_until action, so there is no need to toggle it back to
+ # False
+ return True
+ return False
+
+ def alert_response_cb(self, msg):
+ if not self.alert:
+ return
+ if msg.id != self.alert.id:
+ return
+
+ if msg.response != 'ready':
+ self.context.node.get_logger().info(
+ f'Robot [{self.context.name}] received invalid response '
+ f'inside alert response: [{msg.response}], ignoring...'
+ )
+ return
+
+ self.context.node.get_logger().info(
+ f'Received move off signal, robot [{self.context.name}] is done '
+ f'waiting, marking action as completed.'
+ )
+ with self.mutex:
+ self.ready = True
diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_wait_until.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_wait_until.py
new file mode 100644
index 0000000..deb9c49
--- /dev/null
+++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/rmf_wait_until.py
@@ -0,0 +1,483 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import time
+import importlib
+import requests
+from urllib.error import HTTPError
+
+from fleet_adapter_mir_actions.mir_action import MirAction, MirActionFactory
+from fleet_adapter_mir.robot_adapter_mir import ActionContext
+
+
+class ActionFactory(MirActionFactory):
+ def __init__(self, context: ActionContext):
+ MirActionFactory.__init__(self, context)
+ self.custom_modules = {}
+ # Raise error if config file is invalid
+ supported_signal_types = {'mission', 'plc', 'custom'}
+ signals = context.action_config.get('signals')
+ if signals is not None:
+ for signal_name, signal_config in signals.items():
+ signal_type = signal_config.get('signal_type')
+ if signal_type is None:
+ raise KeyError(
+ f'WaitUntil MirAction signal config requires a '
+ f'defined signal_type, but signal_type is not '
+ f'provided for {signal_name}! Unable to instantiate '
+ f'WaitUntil MirActionFactory.'
+ )
+ elif signal_type not in supported_signal_types:
+ raise ValueError(
+ f'WaitUntil MirAction signal config requires a '
+ f'defined signal_type, but signal_type provided '
+ f'{signal_name} is not supported! We currently '
+ f'support the following signal types: '
+ f'{supported_signal_types}. Unable to instantiate '
+ f'WaitUntil MirActionFactory.'
+ )
+
+ # Pass signal config into verify functions to validate types
+ # and catch missing values
+ match signal_type:
+ case 'mission':
+ self.verify_mission(
+ signal_config.get('mission_name'),
+ signal_config.get('retry_count'),
+ signal_config.get('resubmit_on_abort')
+ )
+ case 'plc':
+ self.verify_plc(signal_config.get('register'))
+ case 'custom':
+ self.verify_custom_module(signal_config.get('module'))
+ else:
+ # If the user did not provide any valid signal config, log a
+ # warning to remind users to provide signal type config in any task
+ # description they submit
+ self.context.node.get_logger().warn(
+ f'WaitUntil ActionFactory is instantiated for robot '
+ f'[{self.context.name}], but no valid signals config has been '
+ f'provided in the action config! Any move off signal config '
+ f'will have to be provided in the task description submitted '
+ f'to RMF.'
+ )
+ return
+
+ # Register default signal type
+ default_signal = context.action_config.get('default_signal')
+ if default_signal is not None:
+ if default_signal not in supported_signal_types:
+ raise ValueError(
+ f'User provided a default signal type {default_signal} '
+ f'in the action config for WaitUntil MirAction, but the '
+ f'default signal type is not supported!')
+ elif default_signal not in signals.keys():
+ raise ValueError(
+ f'User provided a default signal type {default_signal} '
+ f'in the action config for WaitUntil MirAction, but the '
+ f'default signal type is not configured!')
+ else:
+ self.context.node.get_logger().warn(
+ f'WaitUntil ActionFactory instantiated for robot '
+ f'[{self.context.name}], but no default signal type has been '
+ f'provided in the action config! If no signal type is '
+ f'populated in the task description, there will be no move '
+ f'off signal available and the robot will wait for the full '
+ f'duration of the timeout when this action is triggered.'
+ )
+
+ def supports_action(self, category: str) -> bool:
+ match category:
+ case 'wait_until':
+ return True
+ case _:
+ return False
+
+ def perform_action(
+ self,
+ category: str,
+ description: dict,
+ execution
+ ) -> MirAction:
+ match category:
+ case 'wait_until':
+ return WaitUntil(
+ description, execution, self.context,
+ self.custom_modules)
+
+ def verify_mission(
+ self,
+ signal_name: str,
+ mission_name: str | None,
+ retry_count: int | None,
+ resubmit_on_abort: bool | None):
+ if mission_name is None:
+ raise KeyError(
+ f'WaitUntil MirAction requires a mission name for the '
+ f'[mission] signal type, but mission name is not provided in '
+ f'the action config for {signal_name}! Unable to instantiate '
+ f'WaitUntil MirActionFactory.'
+ )
+ if retry_count is not None and retry_count < 0:
+ self.context.node.warn(
+ f'WaitUntil MirAction takes in a retry count for the '
+ f'[mission] signal type, but the value provided '
+ f'[{retry_count}] is invalid! Ignoring provided value and '
+ f'using the default retry count of 10 for {signal_name}.'
+ )
+
+ def verify_plc(self, signal_name: str, plc_register: int | None):
+ if plc_register is None:
+ raise KeyError(
+ f'WaitUntil MirAction requires a default PLC register for '
+ f'signal type [plc], but no PLC register was provided in the '
+ f'action config for {signal_name}! Unable to instantiate '
+ f'WaitUntil MirActionFactory.'
+ )
+
+ def verify_custom_module(
+ self,
+ signal_name: str,
+ module: str | None):
+ if module is None:
+ raise KeyError(
+ f'WaitUntil MirAction requires a custom module for the '
+ f'[custom] signal type, but path to module is not provided in '
+ f'the action config for {signal_name}! Unable to instantiate '
+ f'WaitUntil MirActionFactory.'
+ )
+ try:
+ move_off_plugin = importlib.import_module(module)
+ self.custom_modules[signal_name] = \
+ move_off_plugin.MoveOff(self.context)
+ except ImportError:
+ self.context.node.get_logger().warn(
+ f'Unable to import module {module}! Unable to instantiate '
+ f'WaitUntil MirActionFactory.'
+ )
+
+
+class WaitUntil(MirAction):
+ def __init__(
+ self,
+ description: dict,
+ execution,
+ context: ActionContext,
+ custom_modules: dict
+ ):
+ MirAction.__init__(self, context, execution)
+
+ self.move_off_cb = self.create_move_off_cb(description, custom_modules)
+ if self.move_off_cb is None:
+ # Insufficient information provided to configure the check move off
+ # callback, mark action as completed and continue task
+ self.context.node.get_logger().info(
+ f'Insufficient information provided to configure the move-'
+ f'off behavior for robot [{self.context.name}], cancelling '
+ f'task...'
+ )
+ self.cancel_task(
+ label='Move off behavior cannot be configured, unable to '
+ 'perform wait until action.'
+ )
+ return
+
+ self.update_gap = description.get(
+ 'update_gap',
+ context.action_config.get('update_gap', 60)) # seconds
+ self.wait_timeout = description.get(
+ 'timeout', context.action_config.get('timeout', 60)) # seconds
+ self.signal_config = context.action_config.get('signals')
+ self.default_signal = context.action_config.get('default_signal')
+
+ self.start_time = self.context.node.get_clock().now().nanoseconds / 1e9
+ self.context.node.get_logger().info(
+ f'New wait until action with a timeout of {self.wait_timeout} '
+ f'seconds requested for robot [{self.context.name}]')
+
+ def cancel_task(self, label: str = ''):
+ def _cancel_success():
+ pass
+
+ def _cancel_fail():
+ pass
+ self.cancel_task_of_action(_cancel_success, _cancel_fail, label)
+
+ def update_action(self):
+ # If the action is no longer active, mark action as complete
+ if self.execution is not None and not self.execution.okay():
+ self.context.node.get_logger().info(
+ f'[wait_until] action is no longer underway and valid, '
+ f'marking action as complete.')
+ return True
+
+ # Check if we've received a move off signal
+ if self.move_off_cb():
+ self.context.node.get_logger().info(
+ f'[{self.context.name}] has received move off signal, marking '
+ f'action as completed.'
+ )
+ self.execution.finished()
+ return True
+
+ # Check if the default timeout has passed
+ now = self.context.node.get_clock().now().nanoseconds / 1e9
+ if now > self.start_time + self.wait_timeout:
+ self.context.node.get_logger().info(
+ f'Robot [{self.context.name}] has completed waiting for '
+ f'{self.wait_timeout} seconds without move off signal, '
+ f'action as complete.'
+ )
+ return True
+
+ # Log the robot waiting every X seconds
+ seconds_passed = round(now - self.start_time)
+ if seconds_passed % self.update_gap == 0:
+ self.context.node.get_logger().info(
+ f'{seconds_passed} seconds have passed since robot '
+ f'[{self.context.name}] started its waiting action.'
+ )
+
+ return False
+
+ def create_move_off_cb(self, description: dict, custom_modules: dict):
+ signal_cb = None
+ signal_type = None
+
+ # Determine which move off signal config to use. Any config populated
+ # in the task description overrides the default config provided in
+ # action config.
+ if 'signal_type' in description:
+ signal_type = description['signal_type']
+ elif self.default_signal is not None:
+ signal_type = self.default_signal
+ else:
+ # There is no move off signal provided, we will just wait for the
+ # duration of the configured timeout
+ timeout = description.get('timeout', self.wait_timeout)
+ self.context.node.get_logger().info(
+ f'No move off signal was configured for [{self.context.name}]'
+ f', the robot will begin waiting until the configured timeout '
+ f'of [{timeout}] seconds.'
+ )
+ signal_cb = lambda: False
+ return signal_cb
+
+
+ signal_config = None
+ if signal_type in self.signal_config:
+ # Signal is preconfigured in the fleet action config
+ signal_config = self.signal_config[signal_type]
+ signal_type = signal_config['signal_type']
+ else:
+ signal_config = description.get('signal_config')
+ if signal_config is None:
+ self.context.node.get_logger().error(
+ f'The submitted signal type/config is invalid for '
+ f'{signal_type}!'
+ )
+ return None
+
+ match signal_type:
+ case 'mission':
+ mission_name = signal_config['mission_name']
+ resubmit_on_abort = signal_config.get(
+ 'resubmit_on_abort', False)
+ retry_count = signal_config.get('retry_count', 10)
+ signal_cb = self.create_mission_move_off_cb(
+ mission_name, retry_count, resubmit_on_abort)
+ case 'plc':
+ register = signal_config['register']
+ self.context.node.get_logger().info(
+ f'Configuring robot [{self.context.name}] move off '
+ f'signal: robot will wait until the PLC register '
+ f'{register} returns True.'
+ )
+ signal_cb = lambda: self.check_plc_register(register)
+ case 'custom':
+ module = custom_modules.get(signal_type)
+ if module is None:
+ self.context.node.get_logger().info(
+ f'MoveOff signal type [custom] was selected for '
+ f'robot [{self.context.name}], but no valid move off '
+ f'signal module was provided! Please ensure that the '
+ f'required fields are provided in the fleet config.'
+ )
+ return None
+ module.begin_waiting(description)
+ self.context.node.get_logger().info(
+ f'Configuring robot [{self.context.name}] move off '
+ f'behavior: robot will wait until the custom move off '
+ f'behavior signals that the robot is ready to move off.'
+ )
+ signal_cb = lambda: module.is_move_off_ready()
+ case _:
+ self.context.node.get_logger().info(
+ f'Invalid move off signal type [{signal_type}] provided, '
+ f'unable to initialize a WaitUntil action for '
+ f'{self.context.name}.'
+ )
+ return signal_cb
+
+ def create_mission_move_off_cb(
+ self,
+ mission_name: str,
+ retry_count: int,
+ resubmit_on_abort: bool):
+ if mission_name not in self.context.api.known_missions:
+ self.context.node.get_logger().info(
+ f'Mission {mission_name} not found on robot '
+ f'{self.context.name}!'
+ )
+ return None
+ mission_actions = \
+ self.context.api.missions_mission_id_actions_get(
+ self.context.api.known_missions[mission_name]['guid']
+ )
+ if not mission_actions:
+ self.context.node.get_logger().info(
+ f'Mission {mission_name} actions not found on robot '
+ f'{self.context.name}!'
+ )
+ return None
+ # Queue the waiting mission for this robot
+ count = 0 # we should attempt (retry_count + 1) times in total
+ mission_queue_id = None
+ while count <= retry_count and not mission_queue_id:
+ count += 1
+ self.context.node.get_logger().info(
+ f'Queueing mission {mission_name} for robot '
+ f'[{self.context.name}]...'
+ )
+ try:
+ mission_queue_id = \
+ self.context.api.queue_mission_by_name(
+ mission_name)
+ if mission_queue_id is not None:
+ break
+ except Exception as err:
+ self.context.node.get_logger().info(
+ f'Failed to queue mission {mission_name}: {err}. '
+ f'Retrying...'
+ )
+ time.sleep(1)
+ if not mission_queue_id:
+ self.context.node.get_logger().info(
+ f'Unable to queue mission {mission_name} for robot '
+ f'[{self.context.name}]!'
+ )
+ return None
+ self.context.node.get_logger().info(
+ f'Mission {mission_name} queued for [{self.context.name}] '
+ f'with mission queue id {mission_queue_id}.'
+ )
+ self.context.node.get_logger().info(
+ f'Configuring robot [{self.context.name}] move off signal'
+ f': robot will wait until mission {mission_name} with '
+ f'mission queue id {mission_queue_id} is completed.'
+ )
+ return lambda: self.check_mission_complete(
+ mission_name, mission_queue_id, resubmit_on_abort)
+
+ def check_mission_complete(
+ self,
+ mission_name,
+ mission_queue_id,
+ resubmit_on_abort
+ ):
+ mission_status = \
+ self.context.api.mission_queue_id_get(mission_queue_id)
+ if (mission_status is not None and
+ mission_status['state'] == 'Done'):
+ # Mission has completed, we can set move_off to True
+ self.context.node.get_logger().info(
+ f'Robot [{self.context.name}] has completed its mission '
+ f'with mission queue id {mission_queue_id}'
+ )
+ return True
+
+ if (mission_status is not None and
+ mission_status['state'] == 'Aborted'):
+ if resubmit_on_abort:
+ # Mission aborted for some reason, let's submit the mission
+ # again
+ new_mission_queue_id = self.context.api.queue_mission_by_name(
+ mission_name)
+ if not new_mission_queue_id:
+ # If we didn't successfully post a new mission, we'll
+ # try again in the next update_action loop
+ return
+ # Update the check move off callback with the updated mission
+ # queue id
+ self.move_off_cb = lambda: self.check_mission_complete(
+ mission_name,
+ new_mission_queue_id)
+ self.context.node.get_logger().info(
+ f'Robot [{self.context.name}] aborted mission with queue '
+ f'id {mission_queue_id}, re-submitting mission with new '
+ f'queue id {new_mission_queue_id}'
+ )
+ else:
+ # If mission is aborted without option to resubmit on abort,
+ # mark mission as finished
+ self.context.node.get_logger().info(
+ f'Robot [{self.context.name}] has aborted its mission '
+ f'with mission queue id {mission_queue_id}, marking '
+ f'action as completed.'
+ )
+ return True
+ return False
+
+ def check_plc_register(self, register: int):
+ # Update register to check if PLC register returns a non-zero value
+ value = self.register_get(register)
+ if value:
+ self.context.node.get_logger().info(
+ f'[{self.context.name}] PLC register {register} detected '
+ f'value {value}, robot is ready to move off.'
+ )
+ return True
+ return False
+
+ # --------------------------------------------------------------------------
+ # HELPFUL FUNCTIONS FOR INTERACTING WITH MIR REST API
+ # --------------------------------------------------------------------------
+
+ def register_get(self, register: int) -> int:
+ if not self.context.api.connected:
+ return None
+ try:
+ response = requests.get(
+ self.context.api.prefix + f'registers/{register}',
+ headers=self.context.api.headers,
+ timeout=self.context.api.timeout)
+ if self.context.api.debug:
+ print(f"Response: {response.headers}")
+ value = response.json().get('value', 0)
+ # Convert value into int if required
+ if isinstance(value, str):
+ try:
+ return int(value)
+ except ValueError as value_err:
+ print(f"Value error: {value_err}")
+ return None
+ elif isinstance(value, int):
+ return value
+ return None
+ except HTTPError as http_err:
+ print(f"HTTP error: {http_err}")
+ return None
+ except Exception as err:
+ print(f"Other error: {err}")
+ return None
diff --git a/fleet_adapter_mir_actions/fleet_adapter_mir_actions/test_rmf_cart_detection.py b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/test_rmf_cart_detection.py
new file mode 100644
index 0000000..f703d47
--- /dev/null
+++ b/fleet_adapter_mir_actions/fleet_adapter_mir_actions/test_rmf_cart_detection.py
@@ -0,0 +1,50 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from fleet_adapter_mir.robot_adapter_mir import ActionContext
+from .rmf_cart_detection import BaseCartDetection
+
+
+class CartDetection(BaseCartDetection):
+ def __init__(self, context: ActionContext):
+ BaseCartDetection.__init__(self, context)
+
+ '''
+ This method checks if the robot's latch is open and carrying a cart.
+ Return True if latch is open, else False.
+ '''
+ def is_latch_open(self):
+ return self.register_get(20)
+
+ '''
+ This method checks if the robot is currently docked under a cart.
+ Return True if robot is under any carts, else False.
+ '''
+ def is_under_cart(self):
+ detected_cart_id = self.register_get(30)
+ if (detected_cart_id is None or detected_cart_id == 0):
+ return False
+ return True
+
+ '''
+ This method checks if the detected cart identifier matches the target
+ cart_id, if any.
+ Return True if cart is correct, False if cart is wrong, None if no cart is
+ detected.
+ '''
+ def is_correct_cart(self, cart_id: str | None):
+ if cart_id is None or cart_id == '':
+ return True
+ detected_cart_id = self.register_get(30)
+ return detected_cart_id == int(cart_id)
diff --git a/fleet_adapter_mir_tasks/README.md b/fleet_adapter_mir_tasks/README.md
new file mode 100644
index 0000000..e69de29
diff --git a/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_multistop.py b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_multistop.py
new file mode 100644
index 0000000..337140e
--- /dev/null
+++ b/fleet_adapter_mir_tasks/fleet_adapter_mir_tasks/dispatch_multistop.py
@@ -0,0 +1,229 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import sys
+import uuid
+import argparse
+import json
+import asyncio
+
+import rclpy
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.qos import qos_profile_system_default
+from rclpy.qos import QoSProfile
+from rclpy.qos import QoSHistoryPolicy as History
+from rclpy.qos import QoSDurabilityPolicy as Durability
+from rclpy.qos import QoSReliabilityPolicy as Reliability
+
+from rmf_task_msgs.msg import ApiRequest, ApiResponse
+
+
+###############################################################################
+
+class TaskRequester(Node):
+
+ def __init__(self, argv=sys.argv):
+ super().__init__('task_requester')
+ parser = argparse.ArgumentParser()
+ parser.add_argument('-F', '--fleet', required=False, default='',
+ type=str, help='Fleet name')
+ parser.add_argument('-R', '--robot', required=False, default='',
+ type=str, help='Robot name')
+ parser.add_argument('-g', '--go_to', required=True, nargs='+',
+ type=str,
+ help='Places to go to for multistop task')
+ parser.add_argument('-t', '--timeout', type=int,
+ help='Number of seconds to timeout')
+ parser.add_argument('-u', '--update_gap', type=int,
+ help='Number of seconds between logging updates')
+ parser.add_argument('-s', '--signal_type', type=str,
+ help='Move off signal type')
+ parser.add_argument('-m', '--mission_name', type=str,
+ help='Mission name')
+ parser.add_argument('-r', '--resubmit_on_abort', type=bool,
+ help='Resubmit mission if aborted by robot')
+ parser.add_argument('-rc', '--retry_count', required=False, default=-1,
+ type=int,
+ help='Number of retries to queue mission')
+ parser.add_argument('-p', '--plc_register', type=int,
+ help='PLC register number')
+ parser.add_argument('-st', '--start_time',
+ help='Start time from now in secs, default: 0',
+ type=int, default=0)
+ parser.add_argument('-pt', '--priority',
+ help='Priority value for this request',
+ type=int, default=0)
+ parser.add_argument("--use_sim_time", action="store_true",
+ help='Use sim time, default: false')
+
+ self.args = parser.parse_args(argv[1:])
+ self.response = asyncio.Future()
+
+ transient_qos = QoSProfile(
+ history=History.KEEP_LAST,
+ depth=1,
+ reliability=Reliability.RELIABLE,
+ durability=Durability.TRANSIENT_LOCAL)
+
+ self.pub = self.create_publisher(
+ ApiRequest, 'task_api_requests', transient_qos)
+
+ # enable ros sim time
+ if self.args.use_sim_time:
+ self.get_logger().info("Using Sim Time")
+ param = Parameter("use_sim_time", Parameter.Type.BOOL, True)
+ self.set_parameters([param])
+
+ # Construct task
+ msg = ApiRequest()
+ msg.request_id = f"multistop_" + str(uuid.uuid4())
+ payload = {}
+ if self.args.fleet and self.args.robot:
+ payload["type"] = "robot_task_request"
+ payload["robot"] = self.args.robot
+ payload["fleet"] = self.args.fleet
+ else:
+ payload["type"] = "dispatch_task_request"
+ request = {}
+
+ # Set task request start time
+ now = self.get_clock().now().to_msg()
+ now.sec = now.sec + self.args.start_time
+ start_time = now.sec * 1000 + round(now.nanosec/10**6)
+ request["unix_millis_earliest_start_time"] = start_time
+
+ # Define task request category
+ request["category"] = "compose"
+
+ # Define task request description with phases
+ description = {} # task_description_Compose.json
+ description["category"] = "rmf_multistop"
+ description["phases"] = []
+
+ # GoToPlace + wait
+ for place in self.args.go_to:
+ # Add GoToPlace activity
+ go_to_place_activity = [{
+ "category": "go_to_place",
+ "description": place
+ }]
+ description["phases"].append({
+ "activity": {
+ "category": "sequence",
+ "description": {"activities": go_to_place_activity}
+ }
+ })
+ # Configure wait_until description
+ signal_type = self.args.signal_type
+ signal_config = {}
+ match signal_type:
+ case "mission":
+ if self.args.mission_name is None:
+ raise ValueError(
+ f'No mission name provided for [mission] signal '
+ f'type!'
+ )
+ signal_config['mission_name'] = self.args.mission_name
+ if self.args.resubmit_on_abort is not None:
+ signal_config['resubmit_on_abort'] = \
+ self.args.resubmit_on_abort
+ if self.args.retry_count > -1:
+ signal_config['retry_count'] = self.args.retry_count
+ case "plc":
+ if self.args.plc_register is None:
+ raise ValueError(
+ f'No PLC register provided for [plc] signal type!'
+ )
+ signal_config['register'] = self.args.plc_register
+ case "custom":
+ raise ValueError(
+ f'[custom] signal type is not supported via task '
+ f'description! Please provide the path to module in '
+ f'the fleet action config.'
+ )
+ case _:
+ # The signal type provided, if valid, points to a
+ # configured signal type. We pass it to the action for
+ # validation.
+ pass
+ # Add wait activity
+ wait_activity = [{
+ "category": "perform_action",
+ "description": {
+ "unix_millis_action_duration_estimate": 60000,
+ "category": 'wait_until',
+ "description": {
+ "signal_config": signal_config
+ }
+ }
+ }]
+ # We only add in these parameters if they are specified and valid
+ # The perform action plugin will use the default values if these
+ # are not provided
+ if signal_type is not None:
+ wait_activity[0]['description']['description']['signal_type'] = \
+ signal_type
+ if self.args.timeout is not None:
+ wait_activity[0]['description']['description']['timeout'] = \
+ self.args.timeout
+ if self.args.update_gap is not None:
+ wait_activity[0]['description']['description']['update_gap'] = \
+ self.args.update_gap
+ description["phases"].append({
+ "activity": {
+ "category": "sequence",
+ "description": {
+ "activities": wait_activity
+ }}})
+
+ # Consolidate
+ request["description"] = description
+ payload["request"] = request
+ msg.json_msg = json.dumps(payload)
+
+ def receive_response(response_msg: ApiResponse):
+ if response_msg.request_id == msg.request_id:
+ self.response.set_result(json.loads(response_msg.json_msg))
+
+ transient_qos.depth = 10
+ self.sub = self.create_subscription(
+ ApiResponse, 'task_api_responses', receive_response, transient_qos
+ )
+
+ print(f"Json msg payload: \n{json.dumps(payload, indent=2)}")
+ self.pub.publish(msg)
+
+
+###############################################################################
+
+
+def main(argv=sys.argv):
+ rclpy.init(args=sys.argv)
+ args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
+
+ task_requester = TaskRequester(args_without_ros)
+ rclpy.spin_until_future_complete(
+ task_requester, task_requester.response, timeout_sec=5.0)
+ if task_requester.response.done():
+ print(f'Got response:\n{task_requester.response.result()}')
+ else:
+ print('Did not get a response')
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main(sys.argv)
diff --git a/fleet_adapter_mir_tasks/setup.py b/fleet_adapter_mir_tasks/setup.py
index 2502dbf..687cbce 100644
--- a/fleet_adapter_mir_tasks/setup.py
+++ b/fleet_adapter_mir_tasks/setup.py
@@ -19,7 +19,8 @@
license='Apache License 2.0',
entry_points={
'console_scripts': [
- 'dispatch_delivery = fleet_adapter_mir_tasks.dispatch_delivery:main'
+ 'dispatch_delivery = fleet_adapter_mir_tasks.dispatch_delivery:main',
+ 'dispatch_multistop = fleet_adapter_mir_tasks.dispatch_multistop:main'
],
},
)
diff --git a/missions/rmf_cart_missions.json b/missions/rmf_cart_missions.json
index 602913d..b33bf7a 100644
--- a/missions/rmf_cart_missions.json
+++ b/missions/rmf_cart_missions.json
@@ -65,4 +65,4 @@
"action_type": "relative_move"
}
]
-}
\ No newline at end of file
+}