revamp v2

.gitignore

@@ -0,0 +1 @@
+__pycache__

README.md

@@ -9,6 +9,7 @@ For more information on the APIs, please reference [this document](http://downlo
 
 ## Requirements
 * [Open-RMF](https://github.com/open-rmf/rmf)
+* nudged: `pip3 install nudged`
 
 ## Setup
 * Clone this repository into a workspace
@@ -20,22 +21,76 @@ Ensure the robot can be pinged.
 
 ```
 ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH
+```
 
+To run the fleet adapter with [rmf-web](https://github.com/open-rmf/rmf-web/), specify the server_uri (`-s`):
+```bash
+ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH -s ws://localhost:8000/_internal
 ```
 
-## Test the fleet adapter in simulation
-The adapter can be tested in simulation with the help of the [ecobot_sim_server](fleet_adapter_ecobot/ecobot_sim_server.py).
-This script emulates the API server of the robot and can be used to command robots in simulation to follow commands from Open-RMF.
+## Test the fleet adapter in "Mock Mode"
+The adapter can be tested in mock mode with the help of the [TestClientAPI](fleet_adapter_ecobot/TestClientAPI.py). This class emulates the member functions of `EcobotClientAPI.py` which calls the rest apis of the robot. This "mock mode" is enabled by providing `-tf` argument.
+
+![](../media/media/office-world-rviz.png)
 
-First run the server
+Run this example in office world:
+```bash
+ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0
 ```
-ros2 run fleet_adapter_ecobot ecobot_sim_server -c CONFIG_FILE -n NAV_GRAPH -p PORT
 
+Then run the ecobot fleet adapter
+```bash
+# Note the addition of "--test_api_config_file" and "-tf"
+ros2 run fleet_adapter_ecobot fleet_adapter_ecobot \
+    -c src/fleet_adapter_ecobot/configs/robot_config.yaml \
+    -n install/rmf_demos_maps/share/rmf_demos_maps/maps/office/nav_graphs/0.yaml \
+    -s "ws://localhost:8000/_internal" \
+    -tf src/fleet_adapter_ecobot/configs/test_api_config.yaml
+```
+
+Different to the simulation running on gazebo, this `TestClientAPI` mocks the behavior of the fleet adapter when receives command from RMF. Thus, the script is running on actual system wall time.
+
+### Patrol Task
+
+Now try to command robot to move to `Pantry`
+```bash
+ros2 run rmf_demos_tasks dispatch_patrol -p pantry
+```
+
+### Custom Clean Task
+
+Send the robot to clean an area. This custom clean task is created by composing `go_to_place` and custom `perform_action`.
+```bash
+ros2 run rmf_demos_tasks dispatch_action -s patrol_D2 -a clean -ad '{ "clean_task_name": "clean_hallway" }'
 ```
 
-Then run the fleet adapter
+### Show overlayed ecobot map
+Show overlayed lidar map on rviz2 office map
+```bash
+ros2 launch rmf_demos map_server.launch.py map_name:=ecobot_office tx:=1.33 ty:=0.057 yaw:=-1.598
 ```
-ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH --USE_SIM_TIME
+
+### Docking to Charger
+
+Add a `dock_name` on a charger waypoint in traffic editor. This will then call the `dock()` function when the robot is approaching the charger. Note that this is not demonstrated in this demo.
+
+## Get the Ecobot to RMF map Transformation with `traffic-editor`
+
+To get the transformation of the robot map to rmf map, user can add a "floorplan" of a robot map. Then annotate and the corresponding "constraint-pairs", lastly `ctrl-T` to let traffic-editor calculate the respective transformation.
+
+![](../media/media/traffic-editor-transform.png)
+
+Specify this transformation to the `rmf_transform` in the `config.yaml` file. Note that the value of Y-axis is applied with a -ve.
+
+
+Then if you wish to configure your custom waypoints in the `configs/test_api_config.yaml`, you can use rviz to obtain those points in ecobot coordinates. Run this on a separate terminal.
+```bash
+# first run the office map
+ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0
+# then run this on a seperate terminal
+ros2 run fleet_adapter_ecobot clicked_point_transform -tf 1.33 0.057 -1.598 0.049
 ```
 
-Ensure that the `base_url` in the config matches the `LOCALHOST:PORT` specified to the server.
+![](../media/media/rviz2_publish_point.png)
+
+Subsequently, select "publish point" on rviz, then click on the respective point on the map. Immediately, the point in rmf and robot coordinate will get printed on `clicked_point_transform` terminal. These coordinates are helpful during debugging.

configs/robot_config.yaml

@@ -0,0 +1,76 @@
+# FLEET CONFIG =================================================================
+# RMF Fleet parameters
+
+rmf_fleet:
+  name: "ecobot40"
+  limits:
+    linear: [1.2, 1.5] # velocity, acceleration
+    angular: [0.6, 0.6]
+  profile: # Robot profile is modelled as a circle
+    footprint: 0.5
+    vicinity: 0.6
+  reversible: False
+  battery_system:
+    voltage: 24.0
+    capacity: 40.0
+    charging_current: 26.4
+  mechanical_system:
+    mass: 80.0
+    moment_of_inertia: 20.0
+    friction_coefficient: 0.20
+  ambient_system:
+    power: 20.0
+  cleaning_system:
+    power: 760.0
+  recharge_threshold: 0.05
+  recharge_soc: 1.0
+  publish_fleet_state: True
+  account_for_battery_drain: True
+  task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing
+    loop: True
+    delivery: False
+    clean: False # deprecated, now replace as custom action
+    finishing_request: "nothing"
+    action_categories: ["clean", "manual_control"]
+
+# Ecobot CONFIG =================================================================
+
+robots:
+  ecobot40_1:
+    ecobot_config:
+      base_url: "http://10.7.5.88:8080"
+      max_delay: 30.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned
+      filter_waypoints: True
+      cleaning_task_prefix: "" # the prefix of the cleaning task created
+      active_cleaning_config: "light_cleaning" # the cleaning config used during cleaning
+      inactive_cleaning_config: "no_cleaning" # the cleaning config used during navigation
+    rmf_config:
+      robot_state_update_frequency: 0.5
+      max_merge_lane_distance: 15.0 # means how far will the robot diverge from the defined graph
+      charger:
+        waypoint: "tinyRobot1_charger"
+
+# TRANSFORM CONFIG =============================================================
+# For computing transforms between Ecobot and RMF coordinate systems
+
+# TRANSFORM CONFIG =============================================================
+# For computing transforms between Ecobot and RMF coordinate systems
+# Format robot_map: {rmf_map_name: rmf_map, transform: [tx,ty,r,s] }
+# Format robot_map: {rmf_map_name: rmf_map, reference_coordinates: 
+#                                 {rmf: [[x, y]....], rmf: [[x, y]....]}}
+rmf_transform:
+  mock_test_robot_map:
+    rmf_map_name: "L1"
+    transform: [1.33, 0.057, -1.598, 0.049] # This is obtained from traffic-editor
+  ## User can also provide a pair of "reference_coordinates"
+  # mock_test_robot_map2:
+  #   rmf_map_name: "L1"
+  #   reference_coordinates:
+  #     rmf: [[33.11, -18.99],
+  #           [111.3, -19.06],
+  #           [86.16, -19.02],
+  #           [24.68, -19.08] ]
+  #     robot: [ [3876, 741],
+  #               [2293, 703],
+  #               [2816, 710],
+  #               [4058, 747] ]

configs/test_api_config.yaml

@@ -0,0 +1,23 @@
+# note that yaw (x, y, yaw) from gaussian api is in degree, not radian
+# mock_dock_path is empty since dock_name/dock() is not used in this scenario
+
+mock_clean_path: [
+    [60.58, 186.84, 0],
+    [81.59, 169.53, 0],
+    [108.94, 162.66, 45],
+    [137.94, 180.66, 90],
+    [137.01, 226.99, 90],
+    [132.72, 275.21, 90],
+    [131.54, 318.55, 120],
+    [127.56, 355.20, 180],
+    [99.97, 358.42, -140],
+    [66.74, 325.73, -90],
+    [71.04, 289.43, -90],
+    [73.83, 235.87, -90],
+    [77.11, 200.20, -90],
+    [60.58, 186.84, -90]
+]
+mock_dock_path: []
+dock_position: [99.70, 392.85, 0] # charging point
+mock_location: [60.58, 381.85, 0] # this indicate where the robot will start
+mock_robot_map_name: "mock_test_robot_map"