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,51 @@ 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: ["rmf_cart_delivery", "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"
+    class: "CartDelivery"
+    actions: ["rmf_cart_delivery", "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
+      update_footprint: "rmf_update_footprint" # Name of MiR mission for robot to update the footprint of a robot - to be created by fleet adapter upon launch
+    footprints:
+      robot: "MiR100-200" # Name of robot footprint stored on MiR
+      cart: "ShelfCart" # Name of cart footprint stored on MiR
+    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
+    pickup_dist_threshold: 3.0 # metres
+    # Examples of additional fields for cart detection module
+    io_name: "MiR internal IOs"
+    latch_io: 2

fleet_adapter_mir/README.md

@@ -41,8 +41,70 @@ Alternatively, if you want to run everything with full capabilities, (though not
 ros2 run fleet_adapter_mir fleet_adapter_mir -c mir_config.yaml -n nav_graph.yaml
 ```
 
+If you have not configured the necessary RMF missions on the MiR, you may parse the relevant JSON filepaths when launching the fleet adapter node. On startup, the fleet adapter will create these missions via MiR REST API
+
+```bash
+ros2 run fleet_adapter_mir fleet_adapter_mir -c mir_config.yaml -n nav_graph.yaml -a ../missions/rmf_missions.json
+```
+
 
 
 ### Configuration
 
-An example configuration file, `mir_config.yaml` has been provided. It has been generously commented, and in the cases where it has not, the parameter names are self-explanatory enough.
+An example configuration file, `mir_config.yaml` has been provided. It has been generously commented, and in the cases where it has not, the parameter names are self-explanatory enough.
+
+
+
+### Setting up building maps
+
+**Chargers**
+
+To use the `rmf_dock_and_charge` mission for charging, use the traffic-editor to set the `dock_name` property of your charging point to a json description like the following:
+```json
+{"description": {"end_waypoint": "charger_name"}, "mission_name": "rmf_dock_and_charge"}
+```
+Where you replace `end_waypoint` with the name of your MiR charger.
+
+
+**MiR positions**
+
+Upon launch, the MiR fleet adapter recognizes MiR positions with identical names to RMF waypoints to be the same location. Hence, when a navigation command is submitted for the robot to a specific waypoint, if this waypoint name also exists as a robot position on the MiR, the fleet adapter would send it directly to the MiR position even if the coordinates are different.
+
+
+### Plugins
+
+The MiR is capable of performing various types of custom missions and tasks. You can now easily set up plugins offered in this repo instead of writing your own perform action for common use cases. These plugins offered are available under the `fleet_adapter_mir_actions` package.
+
+For cart deliveries from point A to B:
+
+**rmf_cart_delivery**
+
+The `rmf_cart_delivery` plugin allows users to submit pickup and dropoff tasks to MiR integrated with RMF. The workflow of the 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
+3. If the robot successfully docks under the correct cart, it will proceed to deliver it to a dropoff point. If the cart is missing or is not the desired cart, RMF will cancel the task.
+
+Some relevant MiR missions (docking, exit, update footprint) will be automatically created on the MiR on startup. These missions are used to facilitate the pickup and dropoff activities and can be found in the plugin config under `missions`. They are:
+- `rmf_dock_to_cart`: Docks robot under the cart
+- `rmf_exit_lot`: Calls the robot to exit from under the cart
+- `rmf_update_footprint`: Updates the robot footprint
+
+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.
+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 -1 metre in the X-direction)
+2. Fill in the MiR mission names in the plugin config under `missions`. The default mission names are `rmf_pickup_cart` and `rmf_dropoff_cart`.
+3. Fill in the footprints and marker types to be used for your specific robot and cart in the plugin config.
+4. Create your own `CartDetection` plugin. You may use `rmf_cart_detection.py` as a reference for how your plugin module should be implemented. Fill in the logic to check whether the robot's latching module is open in blanks marked `# IMPLEMENT YOUR CODE HERE`. Some API calls to check the MiR's PLC registers and IO modules are provided in case you may want to use them.
+5. In the plugin config, update the `cart_detection_module` field to point to your own written module.
+
+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
+- `-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.