EasyFullControl fleet_adapter_mir and cart delivery PerformAction plugin

README.md

@@ -1,5 +1,5 @@
 # fleet_adapter_mir
-MiR100 and MirFM Fleet Adapter using the https://github.com/open-rmf/rmf_ros2/tree/main/rmf_fleet_adapter_python
+MiR100, 200 and 250 using the https://github.com/open-rmf/rmf_ros2/tree/main/rmf_fleet_adapter_python
 
 
 
@@ -30,7 +30,7 @@ colcon build
 
 ## Description
 
-These packages implement a MiR robot/MiR Fleet command handle that is managed by a fleet adapter in Python. (Along with some helpers.) It can be used to command and manage a fleet of MiR robots using RMF!
+These packages implement a MiR command handle that is managed by a fleet adapter in Python. (Along with some helpers.) It can be used to command and manage a fleet of MiR robots using RMF!
 
 It uses the `rmf_fleet_adapter_python` bindings, which allows for communication with `open-rmf` libraries and ROS2 nodes.
 
@@ -40,12 +40,14 @@ In effect, it interfaces the MiR REST API with `open-rmf`, all without needing t
 
 ### MiR vs. MiR Fleet
 
-Since the MiR robots and MiR Fleet work with different sets of endpoints that serve different functions, both `fleet_adapter_mir` and `fleet_adapter_mirfm` packages are availble to demonstrate RMF integration between MiR100 and MiR Fleet respectively. In addition, the `mir_fleet_client` package provides additional API needed for the MiR Fleet + RMF integration.
+Since the MiR robots and MiR Fleet work with different sets of endpoints that serve different functions, both `fleet_adapter_mir` and `fleet_adapter_mirfm` packages are availble to demonstrate RMF integration between MiR100/200/250 and MiR Fleet respectively. In addition, the `mir_fleet_client` package provides additional API needed for the MiR Fleet + RMF integration.
 
 For users who wish to take advantage of MiR Fleet's centralized control system/interface or do not have access to individual MiR robots, the `fleet_adapter_mirfm` package enables RMF integration with MiR Fleet by obtaining individual MiR robot positions and mission GUIDs via the MiR Fleet API and dispatching commands directly to the robots themselves. The current MiR Fleet API does not provide all the necessary endpoints for RMF to perform its task allocation and traffic deconfliction to the full extent, hence the implementation contains direct communication between RMF and MiR robots as well.
 
 As such, it is recommended to implement the fleet adapter directly with individual MiR robots using `fleet_adapter_mir`.
 
+**NOTE**: `fleet_adapter_mirfm` is currently not being actively supported.
+
 
 
 ## Additional Notes
@@ -83,7 +85,8 @@ We have to deal with the `rmf_traffic` navgraph and the MiR map, with their own
 
 Luckily, we can leverage the `nudged` Python library to compute coordinate transforms between the two sets of coordinates. You just have to make sure to provide equivalent reference points within the two maps so transforms can be computed.
 
-The transform objects are stored in the robot command handle's `self.transforms` member.
+The transform objects are stored in the EasyFullControl fleet adapter handle.
+
 
 
 

configs/mir_config.yaml

@@ -1,73 +1,57 @@
-# ROBOT CONFIG =================================================================
-# To init MiR robot REST APIs and RMF states
-
-robots:
-  # NOTE(CH3): This robot name is different from the MiR robot name
-  # for some reason
-  #
-  # I don't have access to the MiR REST server to test it out
-  # Doesn't seem very configurable from the code side?
-  #
-  # So for now, this name is the name used in the script only.
-  my_test_robot:
-    mir_config:
-      base_url: "http://some.ip.address/api/v2.0.0/"
-      user: "application/json"
-      password: "Basic HASH_OF_PASSWORD_OBTAINED_AT_API_DOCUMENTATION_PAGE"
-      rmf_move_mission: "rmf_default_move_mission" # This mission must be predefined
-      dock_and_charge_mission: "dock_to_charger" # This mission must be predefined
-
-    rmf_config:
-      robot_state_update_frequency: 1
-      start:
-        map_name: "L1"
-        max_merge_waypoint_distance: 3.0
-        max_merge_lane_distance: 3.0
-        # NOTE(CH3):
-        # If you leave these empty, the robot will try to figure it out on init
-        # from the MiR reported location.
-        #
-        # Otherwise, fill BOTH of these! And make sure that the robot starts
-        # ON the waypoint!
-        waypoint_index: 0 # Optional
-        orientation: 0.0 # Optional, radians
-      charger:
-        waypoint: "charger_waypoint"
-
-
-# NODE CONFIG ==================================================================
 # To init the ROS2 node names used in the script
-
 node_names:
   robot_command_handle: "rmf_mir_fleet_command_handler"
   fleet_state_publisher: "rmf_mir_fleet_state_publisher"
-  rmf_fleet_adapter: "TestDeliveryAdapter"
+  rmf_fleet_adapter: "DeliveryAdapter"
 
 
-# TRANSFORM CONFIG =============================================================
-# For computing transforms between MiR and RMF coordinate systems
-#
-# NOTE(CH3): I am assuming that the transform is the same across maps
-# Otherwise... Future me or future someone else, PLEASE KEY THIS PER MAP
-# And also remember to edit the robot.transforms dictionary accordingly!!
+# Conversions between RMF data and MiR data
+conversions:
+  reference_coordinates:
+    L1: # Map name
+      rmf: [[0.0, 0.0],
+            [0.0, 1.0],
+            [1.0, 1.0],
+            [1.0, 0.0]]
+      mir: [[0.0, 0.0],
+            [0.0, 1.0],
+            [1.0, 1.0],
+            [1.0, 0.0]]
+    L2:
+      rmf: [[0.0, 0.0],
+            [0.0, 1.0],
+            [1.0, 1.0],
+            [1.0, 0.0]]
+      mir: [[0.0, 0.0],
+            [0.0, 1.0],
+            [1.0, 1.0],
+            [1.0, 0.0]]
 
-reference_coordinates:
-  rmf: [[26.95, -20.23],
-        [29.26, -22.38],
-        [11.4, -16.48],
-        [12.46, -16.99]]
+  maps:
+    # "RMF map name": "Map name stored on MiR"
+    L1: "Level 1"
+    L2: "Level 2"
 
-  mir: [[7.2, 16.6],
-        [5.15, 18.35],
-        [23, 12.35],
-        [22.05, 12.95]]
+  missions:
+    move: "rmf_move" # This mission must be predefined
+    dock_and_charge: "rmf_dock_and_charge" # This mission must be predefined
+    localize: "rmf_localize"
+    go_to: "rmf_move_to_position"
 
+  lift_positions:
+    lift_1: # Name of lift matching the navigation graph
+      L1:
+        name: "Level_1_Lift" # Name of lift position stored on the robot on this map
+        # tolerance: 0.3
+      L2:
+        name: "Level_2_Lift"
 
-# FLEET CONFIG =================================================================
-# RMF Fleet parameters
+  marker_types:
+    "charger": "Narrow asymmetric MiR500/1000 shelf"
 
+# RMF Fleet parameters
 rmf_fleet:
-  name: "test_fleet"
+  name: "mir"
   limits:
     linear: [0.7, 0.3] # velocity, acceleration
     angular: [1.0, 0.45]
@@ -85,17 +69,45 @@ rmf_fleet:
     friction_coefficient: 0.20
   ambient_system:
     power: 20.0
-  cleaning_system:
+  tool_system:
     power: 650.0
   recharge_threshold: 0.01
   recharge_soc: 1.0
   account_for_battery_drain: True
-  publish_fleet_state: True
-  fleet_state_topic: "fleet_states"
-  fleet_state_publish_frequency: 1
+  publish_fleet_state: 1.0 # Publish frequency (Hz) for the fleet state
   task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing
     loop: True
-    delivery: False
+    delivery: True
     clean: False
-    finishing_request: "nothing" # [park, charge, nothing]
-    action_categories: ["custom_mission_1"]
+  finishing_request: "charge" # [park, charge, nothing]
+  actions: ["delivery_pickup", "delivery_dropoff"]
+  robots:
+    mir_1:
+      charger: "Charger_1"
+      max_merge_waypoint_distance: 0.25
+      max_merge_lane_distance: 1.0
+      mir_config:
+        base_url: "http://some.ip.address/api/v2.0.0/"
+        user: "application/json"
+        password: "Basic HASH_OF_PASSWORD_OBTAINED_AT_API_DOCUMENTATION_PAGE"
+
+  robot_state_update_frequency: 1
+  debug: False # whether to enable debug printouts from EasyFullControl
+
+plugins:
+  rmf_cart_delivery:
+    module: "fleet_adapter_mir_actions.rmf_cart_delivery"
+    actions: ["delivery_pickup", "delivery_dropoff"]
+    cart_detection_module: "fleet_adapter_mir_actions.rmf_cart_detection"
+    missions:
+      dock_to_cart: "rmf_dock_to_cart" # Name of MiR mission for robot to dock under a cart - to be created by fleet adapter upon launch
+      pickup: "rmf_pickup_cart" # Name of MiR mission for robot to latch onto a cart - to be created and filled in by user
+      dropoff: "rmf_dropoff_cart" # Name of MiR mission for robot to unlatch and exit from under a cart - to be created and filled in by user
+      exit_lot: "rmf_exit_lot" # Name of MiR mission for robot to exit from under a cart - to be created by fleet adapter upon launch
+    marker_types:
+      cart: "AsymmetricShelf" # Name of cart marker type
+    missions_json: "/home/some_user/mir_ws/src/fleet_adapter_mir/missions/rmf_cart_missions.json"  # Filepath to RMF cart delivery missions
+    search_timeout: 60 # Dock to cart timeout in seconds
+    # Examples of additional fields for cart detection module
+    io_name: "MiR internal IOs"
+    latch_io: 2

docs/mir_actions.md

@@ -0,0 +1,56 @@
+## MiR Action Plugins
+
+This section lists and elaborates on the MiR actions provided out-of-the-box in this repo.
+
+## Available Action Plugins
+
+* [rmf_cart_delivery](#rmf_cart_delivery)
+
+
+## rmf_cart_delivery
+
+### Overview
+
+The `rmf_cart_delivery` plugin allows users to submit pickup and dropoff tasks for MiR from point A to point B via RMF. The intended workflow of a delivery task is as follows:
+1. RMF will send the robot to the pickup lot.
+2. The robot will attempt to dock under a cart in the pickup lot. If the cart is missing or is not the desired cart, RMF will cancel the task.
+3. If the robot successfully docks under the correct cart, it will latch onto the cart.
+4. With the cart attached, the robot will move to the designated dropoff point.
+5. Upon reaching the dropoff point, the robot will release and exit from under the cart. The task ends once the robot has safely exited.
+
+### Setup
+
+Some relevant MiR missions are pre-defined and can be automatically created on the MiR on startup. These missions are used to facilitate the pickup and dropoff activities. They are defined and stored in the `rmf_cart_missions.json` file and do not require any further configuration.
+
+However, since there are various types of latching methods available for different MiR models, users will need to set up their custom pickup and dropoff missions on the MiR, as well as implement their own `CartDetection` plugin module with the appropriate APIs to detect latching states.
+
+Before setting up, you may want to refer to the [list of RMF missions](https://github.com/open-rmf/fleet_adapter_mir/blob/main/docs/mir_missions.md#RMF-missions-for-rmf_cart_delivery) required for this MiR Action.
+
+Steps for setup:
+
+1. Create 2 missions on the MiR:
+   - `rmf_pickup_cart`: Triggers the robot's latching module to open
+   - `rmf_dropoff_cart`: Triggers the robot's latching module to close and release the cart, then exit from under the cart (relative move in the negative X-direction)
+2. Fill in the MiR mission names in the plugin config under `missions`.
+   - This helps the fleet adapter identify and map the action to the missions you have created earlier.
+   - The recommended mission names are `rmf_pickup_cart` and `rmf_dropoff_cart`, per the instructions in Step 1. However, it is possible to use a different mission name as long as it is indicated accordingly under `missions`.
+3. Fill in the appropriate cart marker type in the plugin config under `marker_type`.
+4. Create your own `CartDetection` plugin.
+   - You are encouraged to use the `CartDetection` class in `rmf_cart_detection.py` as a base for your own module implementation. The class methods will be used by the `CartPickup` and `CartDropoff` Mir Actions. Some API calls to check the MiR's PLC registers and IO modules are provided in case you may want to use them.
+   - In the plugin config, update the `cart_detection_module` field to point to your own written module.
+6. Enable RMF cart missions creation.
+   - If this is your first time setting up the action, and the pre-defined RMF cart missions have not been created on your robot, you will need to provide the filepath to `rmf_cart_missions.json` under `missions_json`.
+
+You can refer to `mir_config.yaml` under the `configs` folder for an example of a filled-in plugin configuration.
+
+
+### Usage
+
+To submit a cart delivery task, you may use the `dispatch_delivery` task script found in the `fleet_adapter_mir_tasks` package:
+```bash
+ros2 run fleet_adapter_mir_tasks dispatch_delivery -g go_to_waypoint -p pickup_lot -d dropoff_lot -c some_cart_id
+```
+- `-g`: Takes in an existing waypoint name for the robot to travel to before performing the pickup. The robot will begin docking into the pickup lot from this waypoint.
+- `-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. 

docs/mir_missions.md

@@ -0,0 +1,39 @@
+## MiR Missions for RMF
+
+RMF relies on MiR missions to command MiR robots to move and perform various actions. To standardize the missions used for RMF, users have the option to automatically create these missions required by RMF via the fleet adapter by parsing in the JSON filepath. This section lists and explains what these missions do.
+
+Missions may be used for default behavior, which mostly involves the robot patrolling across the same or different maps. Additional missions can be optionally created for custom actions, such as pickup and dropoff missions for delivery tasks.
+
+For more information on how to set up missions, actions and variables, please refer to [official guide documents](https://www.manualslib.com/manual/1941073/Mir-Mir250.html?page=150#manual) (this is for the MiR250).
+
+
+### List of standardized RMF missions
+
+* [Default RMF missions](#Default-RMF-missions)
+* [RMF missions for `rmf_cart_delivery`](#RMF-missions-for-rmf_cart_delivery)
+
+
+### Default RMF missions
+
+Definition can be found in `rmf_missions.json`.
+
+| Mission name   | Use case    | Created by RMF   |
+| -------------- | ----------- | :--------------: |
+| `rmf_dock_and_charge` | Enables the robot to dock into its charger and begin charging | Yes |
+| `rmf_localize` | Switches MiR map once the robot reaches another level | Yes |
+| `rmf_move` | Commands the robot to move to the MiR coordinates `[X, Y, Orientation]` | Yes |
+| `rmf_move_to_position` | Commands the robot to move to the specified MiR Position | Yes |
+| `rmf_follow_line` | Commands the robot to move to two specified MiR Positions in sequence | Yes |
+
+
+### RMF missions for `rmf_cart_delivery`
+
+| Mission name   | Use case    | Created by RMF   |
+| -------------- | ----------- | :--------------: |
+| `rmf_dock_to_cart` | Enables the robot to dock under a cart | Yes |
+| `rmf_exit_lot` | Commands the robot to move backwards by 1 metre, for exiting from under a cart | Yes |
+| `rmf_pickup_cart` | Commands the robot open its latch to attach to a cart | No |
+| `rmf_dropoff_cart` | Commands the robot unlatch from a cart and move backwards to exit from under the cart | No |
+
+**Important note:**
+In order to ensure that your robot has the correct footprint after pickup/dropoff, do remember to add in the appropriate MiR actions to update robot footprint in your `rmf_pickup_cart` and `rmf_dropoff_cart` missions.