From a6ba0095a979d9c1e37bdd403fb53eb6e625b6f0 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Fri, 19 May 2023 16:30:14 +0800 Subject: [PATCH 1/9] Add EFC writeup Signed-off-by: Xi Yu Oh --- src/integration_fleets.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/integration_fleets.md b/src/integration_fleets.md index 933becc..799eed8 100644 --- a/src/integration_fleets.md +++ b/src/integration_fleets.md @@ -1,6 +1,6 @@ # Mobile Robot Fleet Integration -Here we will cover integrating a mobile robot fleet that offers the **Full Control** category of fleet adapter, as discussed in the [RMF Core Overview](./rmf-core.md) chapter. +Here we will cover integrating a mobile robot fleet that offers the **Full Control** category of fleet adapter, as **discussed** in the [RMF Core Overview](./rmf-core.md) chapter. This means we assume the mobile robot fleet manager allows us to specify explicit paths for the robot to follow, and that the path can be interrupted at any time and replaced with a new path. Furthermore, each robot's position will be updated live as the robots are moving. @@ -17,6 +17,8 @@ The C++ API for **Full Control** automated guided vehicle (AGV) fleets can be fo * [`RobotUpdateHandle`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) - Use this to update the position of a robot and to notify the adapter if the robot's progress gets interrupted. * [`RobotCommandHandle`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotCommandHandle.hpp) - This is a pure abstract interface class. The functions of this class must be implemented to call upon the API of the specific fleet manager that is being adapted. +The C++ API for **Easy Full Control** fleets is currently being developed to provide a simple and more accessible way for users to integrate with the Full Control library without having to modify its internal logic. It can be found in the [rmf_fleet_adapter](https://github.com/open-rmf/rmf_ros2/tree/feature/easy_full_control/rmf_fleet_adapter) package of the `rmf_ros2` repo. The [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class contains helpful methods for users to create a `Configuration` object from YAML files containing important fleet configuration parameters and navigation graphs, as well as to make their own fleet adapter with the `Configuration` object. The `add_robot()` method is provided for users to add robots to the new fleet adapter. This method takes in various callbacks that should be written by the user, and will be triggered whenever RMF is retrieving robot state information from the fleet or sending out commands to perform a particular process (navigation, docking, action, etc.). An example of the EasyFullControl fleet adapter can be found in [`easy_fleet_adapter.py`](https://github.com/open-rmf/rmf_demos/blob/feature/easy_full_control/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/easy_fleet_adapter.py) under the `rmf_demos` repo. + The C++ API for **Traffic Light Control** fleets (i.e. fleets that only allow RMF to pause/resume each mobile robot) can also be found in the `rmf_fleet_adapter` package of the `rmf_ros2` repo. The API reuses the `Adapter` class and requires users to initialize their fleet using either of the APIs [here](https://github.com/open-rmf/rmf_ros2/blob/9b4b8a8cc38b323f875a55c70f307446584d1639/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L106-L180). The user has the option to integrate via the [`TrafficLight`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/TrafficLight.hpp) API or for greater convenience, via the [`EasyTrafficLight`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyTrafficLight.hpp) API. The basic workflow of developing a fleet adapter is the following: From b5df89628d7125260e2364e4a67cef4ace98d87d Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Fri, 19 May 2023 18:00:09 +0800 Subject: [PATCH 2/9] Cleanup tutorial page Signed-off-by: Xi Yu Oh --- src/integration_fleets_tutorial.md | 230 ++++++++++++++--------------- 1 file changed, 113 insertions(+), 117 deletions(-) diff --git a/src/integration_fleets_tutorial.md b/src/integration_fleets_tutorial.md index 9055639..295a5da 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -20,174 +20,184 @@ The `fleet_adapter` receives information (position, current ongoing tasks, batte - The fleet adapter will then send the navigation commands to the robot in appropriate API. -> The tutorial provided below is based on the [rmf_demos_fleet_adapter](https://github.com/open-rmf/rmf_demos/tree/main/rmf_demos_fleet_adapter) implemented in the [rmf_demos](https://github.com/open-rmf/rmf_demos) repository. This specific implementation is written in Python and uses REST API as an interface between the fleet adapter and fleet manager.You may choose to use other APIs for your own integration. +> The tutorial provided below is based on the [rmf_demos_fleet_adapter](https://github.com/open-rmf/rmf_demos/tree/main/rmf_demos_fleet_adapter) implemented in the [rmf_demos](https://github.com/open-rmf/rmf_demos) repository. This specific implementation is written in Python and uses REST API as an interface between the fleet adapter and fleet manager. You may choose to use other APIs for your own integration. -## Fetch dependencies +## 1. Pre-requisites -```bash +### Fetch dependencies +Since the `rmf_demos_fleet_adapter` uses REST API as an interface between the fleet adapter and robot fleet manager, we will need to install the required dependencies to use FastAPI. +```bash pip3 install fastapi uvicorn - ``` +### Get started with the fleet adapter template + Clone the [fleet_adapter_template](https://github.com/open-rmf/fleet_adapter_template) repository. -Users can use the template and fill in the missing blocks of code in [`RobotClientAPI.py`](https://github.com/open-rmf/fleet_adapter_template/blob/main/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py) marked with `# IMPLEMENT YOUR CODE HERE #`. This sets up the API between the fleet adapter and the user's robots. +```bash +git clone https://github.com/open-rmf/fleet_adapter_template.git +``` -> The code given below serves as an example for implementing your own fleet adapter using `RobotClientAPI.py`. +This template contains the code for both Full Control and Easy Full Control fleet adapters. Both implementations use API calls in [`RobotClientAPI.py`](https://github.com/open-rmf/fleet_adapter_template/blob/main/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py) to communicate with the robots. -## Update the `config.yaml` file +## 2. Update the `config.yaml` file The `config.yaml` file contains important parameters for setting up the fleet adapter. Users should start by updating these configurations describing their fleet robots. +It is important to stick to the provided fields in the sample `config.yaml` below, otherwise there will be import errors when parsing this YAML file to the fleet adapter. If you would like to edit any of the field names or value range, or even append additional fields, please ensure that you also modify the part of your fleet adapter code that handles this configuration import accordingly. + +Some fields are optional as indicated below. + ```yaml # FLEET CONFIG ================================================================= # RMF Fleet parameters rmf_fleet: - name: "tinyRobot" + name: "DeliveryBot" fleet_manager: - ip: "127.0.0.1" - port: 22011 + prefix: "http://127.0.0.1:8000" user: "some_user" password: "some_password" limits: - linear: [0.5, 0.75] # velocity, acceleration - angular: [0.6, 2.0] # velocity, acceleration + linear: [0.4, 0.2] # velocity, acceleration + angular: [0.3, 0.35] # velocity, acceleration profile: # Robot profile is modelled as a circle - footprint: 0.3 # radius in m - vicinity: 0.5 # radius in m - reversible: True # whether robots in this fleet can reverse + footprint: 0.5 # radius in m + vicinity: 0.6 # radius in m + reversible: False # whether robots in this fleet can reverse battery_system: - voltage: 12.0 # V - capacity: 24.0 # Ahr - charging_current: 5.0 # A + voltage: 24.0 # V + capacity: 40.0 # Ahr + charging_current: 26.4 # A mechanical_system: - mass: 20.0 # kg - moment_of_inertia: 10.0 #kgm^2 - friction_coefficient: 0.22 + mass: 80.0 # kg + moment_of_inertia: 20.0 #kgm^2 + friction_coefficient: 0.20 ambient_system: power: 20.0 # W tool_system: - power: 0.0 # W - recharge_threshold: 0.10 # Battery level below which robots in this fleet will not operate + power: 760.0 # W + recharge_threshold: 0.20 # Battery level below which robots in this fleet will not operate recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks - publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency + max_delay: 10.0 # Allowed seconds of delay of the current itinerary before it gets interrupted and replanned + publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency 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: True + delivery: False clean: False - finishing_request: "park" # [park, charge, nothing] + finishing_request: "nothing" # [park, charge, nothing] + action: [] # Optional, list of performable action names -# TinyRobot CONFIG ================================================================= +# DeliveryBot CONFIG ================================================================= robots: # Here the user is expected to append the configuration for each robot in the # fleet. - # Configuration for tinyRobot1 - tinyRobot1: - robot_config: - max_delay: 15.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned - rmf_config: - robot_state_update_frequency: 10.0 - start: - map_name: "L1" - waypoint: "tinyRobot1_charger" - orientation: 0.0 # radians - charger: - waypoint: "tinyRobot1_charger" - # Configuration for tinyRobot2 - tinyRobot2: + # Configuration for first robot in this fleet + deliverybot1: robot_config: - max_delay: 15.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned + max_delay: 10.0 # Allowed seconds of delay of the current itinerary before it gets interrupted and replanned rmf_config: - robot_state_update_frequency: 10.0 + robot_state_update_frequency: 0.5 start: map_name: "L1" - waypoint: "tinyRobot2_charger" - orientation: 0.0 # radians + # waypoint: "charger_deliverybot1" # Optional + # orientation: 0.0 # Optional, radians + waypoint: null + orientation: null charger: - waypoint: "tinyRobot2_charger" - + waypoint: "charger_deliverybot1" + # Configuration for the second robot in this fleet if there is a second robot + # Uncomment if more than one robot exists. + # deliverybot2: + # robot_config: + # max_delay: 10.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned + # rmf_config: + # robot_state_update_frequency: 0.5 + # start: + # map_name: "L1" + # # waypoint: "charger_deliverybot2" # Optional + # # orientation: 0.0 # Optional, radians + # waypoint: null + # orientation: null + # charger: + # waypoint: "charger_deliverybot2" + +# TRANSFORM CONFIG ============================================================= +# For computing transforms between Robot and RMF coordinate systems + +# Optional reference_coordinates: - rmf: [[20.33, -3.156], [8.908, -2.57], [13.02, -3.601], [21.93, -4.124]] - robot: [[59, 399], [57, 172], [68, 251], [75, 429]] + rmf: [[20.33, -3.156], + [8.908, -2.57], + [13.02, -3.601], + [21.93, -4.124]] + robot: [[59, 399], + [57, 172], + [68, 251], + [75, 429]] ``` -- `rmf_fleet` important fleet parameters including vehicle traits, task capabilities and user information for connecting to the fleet manager. +- `rmf_fleet`: Important fleet parameters including vehicle traits, task capabilities and user information for connecting to the fleet manager. -- `fleet_manager` the prefix, user and password fields that can be configured to suit your chosen API. These parameters will be brought into `RobotClientAPI.py` for you to set up connection with your fleet manager/robots. + - `fleet_manager`: The *prefix*, *user* and *password* fields that can be configured to suit your chosen API. Do make sure to also edit the corresponding fields in `fleet_adapter.py` if you do modify them. These parameters will be brought into `RobotClientAPI.py` for you to set up connection with your fleet manager/robots. -- `limits` maximum values for linear and angular accelerations and velocities. + - `limits`: Maximum values for linear and angular accelerations and velocities. -- `profile` radius of the footprint and personal vicinity of the vehicles in this fleet. + - `profile`: Radius of the footprint and personal vicinity of the vehicles in this fleet. -- `reversible` a flag that can enable/disable reverse traversal in the robot. + - `reversible`: A flag to enable/disable reverse traversal in the robot. -- `battery_system` information about the battery + - `battery_system`: Information about the battery's voltage, capacity and charging current. -- `recharge_threshold` sets a value for minimum charge below which the robot must return to its charger. + - `recharge_threshold`: Sets a value for minimum charge below which the robot must return to its charger. -- `recharge_soc` the fraction of total battery capacity to which the robot should be charged. + - `recharge_soc`: The fraction of total battery capacity to which the robot should be charged. -- `task_capabilities` the tasks that the robot can perform between `loop`, `delivery` and `clean` + - `max_delay`: Allowed seconds of delay of the current itinerary before it gets interrupted and replanned. Note: currently used in `easy_fleet_adapter.py`. -- `finishing_request` what the robot should do when it finishes its task, can be set to `park`, `charge` or `nothing` + - `task_capabilities`: The tasks that the robot can perform between `loop`, `delivery` and `clean`. -- `robots` information about individual fleet robots + - `finishing_request`: What the robot should do when it finishes its task, can be set to `park`, `charge` or `nothing`. -- `tinyRobot1`, `tinyRobot2` name of the robot. + - `action` [Optional]: A list of custom performable actions for the fleet. -- `max_delay` seconds before interruption occurs and replanning happens +- `robots`: Information about each individual robot in the fleet. Each item in this section corresponds to the configuration for a single robot in the fleet. You may add more robots accordingly. -- `robot_state_update_frequency` how frequently should the robot update the fleet + - `deliveryBot1`: Name of the robot. -- `start` specify the starting map name, initial waypoint (x, y) and orientation (in radians) of the robot + - `max_delay`: Allowed seconds before interruption occurs and replanning happens. Note: currently used in `fleet_adapter.py`, to be moved to the `rmf_fleet` section. -- `charger` waypoint name of the robot's charging point + - `robot_state_update_frequency`: How frequently should the robot update the fleet. -- `reference_coordinates` if the fleet robots are not operating in the same coordinate system as RMF, you can provide two sets of (x, y) coordinates that correspond to the same locations in each system. This helps with estimating coordinate transformations from one frame to another. A minimum of 4 matching waypoints is recommended. Note: this is not being implemented in `rmf_demos_fleet_adapter` as the demos robots and RMF are using the same coordinate system. + - `start`: Specifies the starting map name, initial waypoint (x, y) and orientation (in radians) of the robot. -## RobotClientAPI.py + - `charger waypoint`: Name of the robot's charging point. -Users can fill in the appropriate API inside [`RobotClientAPI.py`](https://github.com/open-rmf/fleet_adapter_template/blob/main/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py), which will be used by the `RobotCommandHandle` to make calls to the fleet robots. For example, if your robot uses REST API to interface with the fleet adapter, you will need to make HTTP request calls to the appropriate endpoints within these functions. +- `reference_coordinates` [Optional]: If the fleet robots are not operating in the same coordinate system as RMF, you can provide two sets of (x, y) coordinates that correspond to the same locations in each system. This helps with estimating coordinate transformations from one frame to another. A minimum of 4 matching waypoints is recommended. -```python + Note: this is not being implemented in `rmf_demos_fleet_adapter` as the demos robots and RMF are using the same coordinate system. -import requests -from urllib.error import HTTPError +## 3. Create navigation graphs +A navigation graph is required to be parsed to the fleet adapter so that RMF can understand the robots' environment. They can be created using the [RMF Traffic Editor](https://github.com/open-rmf/rmf_traffic_editor.git) and the [`building_map_generator nav`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp) CLI provided. Refer to the traffic editor repo's README for installation and map generation instructions. -class RobotAPI: - # The constructor below accepts parameters typically required to submit - # http requests. Users should modify the constructor as per the - # requirements of their robot's API - def __init__(self, prefix: str, user: str, password: str): - self.prefix = prefix - self.user = user - self.password = password - self.timeout = 5.0 - self.debug = False +You may also want to look through the [Traffic Editor](./traffic-editor.md) section of this Book for detailed information and instructions on creating your own digital maps. -``` +You should now have a YAML file with information about the lanes and waypoints (among other information) that describe the paths your robot fleet can take. -User must initialize all the essential parameters in the class constructor required for API calls. Extra fields can be added to the constructor if need be -```python +## 4. Fill in your `RobotAPI` - def check_connection(self): - ''' Return True if connection to the robot API server is successful''' - if self.data() is None: - return False - return True +[`RobotClientAPI.py`](https://github.com/open-rmf/fleet_adapter_template/blob/main/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py) provides a set of methods being used by the fleet adapter. These callbacks are triggered when RMF needs to send or retrieve information via the fleet adapter to/from the managed robots. To cater to the interface of your choice, you need to fill in the missing code blocks marked with `# IMPLEMENT YOUR CODE HERE #` within `RobotAPI` with logics to send or retrieve the corresponding information. For example, if your robot uses REST API to interface with the fleet adapter, you will need to make HTTP request calls to the appropriate endpoints within these functions. -``` +Parts of the [`RobotAPI`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py) class implementated for `rmf_demos_fleet_adapter` is further elaborated below to illustrate how you should fill in the missing code: -[`check_connection`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py#L39) will check if connection to the robot was successful +- `position`: Retrieves the robot's current position in its coordinate frame in the format `[x, y, theta]` via a GET request. ```python - def position(self, robot_name: str): ''' Return [x, y, theta] expressed in the robot's coordinate frame or None if any errors are encountered''' @@ -213,13 +223,10 @@ User must initialize all the essential parameters in the class constructor requi except Exception as err: print(f'Other error: {err}') return None - ``` -[`position`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py#L45) function returns the `[x, y, theta]` position of the robot in its coordinate frame - +- `navigate`: Sends a POST request to the robot's fleet manager with the destination coordinates. Returns true if the robot successfully accepts the request, else false. ```python - def navigate(self, robot_name: str, pose, @@ -247,13 +254,11 @@ User must initialize all the essential parameters in the class constructor requi except Exception as err: print(f'Other error: {err}') return False - ``` -[`navigate`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py#L71) Sends an POST request to the robot with the destination coordinates. It returns true if the robot accepts the request, else false. +- `start_process`: Sends a POST request to the robot's fleet manager asking it to perform a task. ```python - def start_process(self, robot_name: str, process: str, @@ -277,7 +282,10 @@ User must initialize all the essential parameters in the class constructor requi except Exception as err: print(f'Other error: {err}') return False +``` +- `stop`: Commands the robot to stop moving via a POST request. +```python def stop(self, robot_name: str): ''' Command the robot to stop. Return True if robot has successfully stopped. Else False''' @@ -294,13 +302,11 @@ User must initialize all the essential parameters in the class constructor requi except Exception as err: print(f'Other error: {err}') return False - ``` -[`start_process`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py#L99) sends a POST request to the robot asking it to perform a task. [`stop`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py#L123) command tells the robot to stop moving. +- `navigation_remaining_duration`: Retrieves the remaining duration for the robot to complete its current navigation. ```python - def navigation_remaining_duration(self, robot_name: str): ''' Return the number of seconds remaining for the robot to reach its destination''' @@ -309,7 +315,11 @@ User must initialize all the essential parameters in the class constructor requi return response['data']['destination_arrival_duration'] else: return 0.0 +``` +- `process_completed`: Checks if the robot has completed the ongoing process or task. This implementation uses an additional `navigation_completed` method to perform a GET request on the process status. + +```python def navigation_completed(self, robot_name: str): ''' Return True if the robot has successfully completed its previous navigation request. Else False.''' @@ -323,20 +333,6 @@ User must initialize all the essential parameters in the class constructor requi ''' Return True if the robot has successfully completed its previous process request. Else False.''' return self.navigation_completed(robot_name) - - def battery_soc(self, robot_name: str): - ''' Return the state of charge of the robot as a value between 0.0 - and 1.0. Else return None if any errors are encountered''' - response = self.data(robot_name) - if response is not None: - return response['data']['battery']/100.0 - else: - return None - ``` -- `navigation_remaining_duration` will return the remaining duration for the robot to complete its current navigation - -- `process_completed` checks if the robot has completed its navigation using the `navigation_completed` function. - -- `battery_soc` will return battery status between 0 and 1.0 +Further parameters may be added to `RobotAPI` to be used in these callbacks if required, such as authentication details and task IDs. You may also wish to write additional methods in `RobotAPI` and call them in your `fleet_adapter.py` for specific use cases. From 22e06b401ba82fa395b10b97386683f92b6ecd18 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Mon, 22 May 2023 17:24:43 +0800 Subject: [PATCH 3/9] Add EFC tutorial Signed-off-by: Xi Yu Oh --- src/integration_fleets.md | 2 +- src/integration_fleets_tutorial.md | 336 ++++++++++++++++++++++++++++- 2 files changed, 336 insertions(+), 2 deletions(-) diff --git a/src/integration_fleets.md b/src/integration_fleets.md index 799eed8..9e0e54b 100644 --- a/src/integration_fleets.md +++ b/src/integration_fleets.md @@ -17,7 +17,7 @@ The C++ API for **Full Control** automated guided vehicle (AGV) fleets can be fo * [`RobotUpdateHandle`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) - Use this to update the position of a robot and to notify the adapter if the robot's progress gets interrupted. * [`RobotCommandHandle`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotCommandHandle.hpp) - This is a pure abstract interface class. The functions of this class must be implemented to call upon the API of the specific fleet manager that is being adapted. -The C++ API for **Easy Full Control** fleets is currently being developed to provide a simple and more accessible way for users to integrate with the Full Control library without having to modify its internal logic. It can be found in the [rmf_fleet_adapter](https://github.com/open-rmf/rmf_ros2/tree/feature/easy_full_control/rmf_fleet_adapter) package of the `rmf_ros2` repo. The [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class contains helpful methods for users to create a `Configuration` object from YAML files containing important fleet configuration parameters and navigation graphs, as well as to make their own fleet adapter with the `Configuration` object. The `add_robot()` method is provided for users to add robots to the new fleet adapter. This method takes in various callbacks that should be written by the user, and will be triggered whenever RMF is retrieving robot state information from the fleet or sending out commands to perform a particular process (navigation, docking, action, etc.). An example of the EasyFullControl fleet adapter can be found in [`easy_fleet_adapter.py`](https://github.com/open-rmf/rmf_demos/blob/feature/easy_full_control/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/easy_fleet_adapter.py) under the `rmf_demos` repo. +The C++ API for **Easy Full Control** fleets provides a simple and more accessible way for users to integrate with the Full Control library without having to modify its internal logic. It can be found in the [rmf_fleet_adapter](https://github.com/open-rmf/rmf_ros2/tree/feature/easy_full_control/rmf_fleet_adapter) package of the `rmf_ros2` repo. The [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class contains helpful methods for users to create a `Configuration` object from YAML files encapsulating important fleet configuration parameters and navigation graphs, as well as to make their own fleet adapter with the `Configuration` object. The `add_robot(~)` method is provided for users to add robots to the new fleet adapter. This method takes in various callbacks that should be written by the user, and will be triggered whenever RMF is retrieving robot state information from the fleet or sending out commands to perform a particular process (navigation, docking, action, etc.). An example of the EasyFullControl fleet adapter can be found in [`easy_fleet_adapter.py`](https://github.com/open-rmf/rmf_demos/blob/feature/easy_full_control/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/easy_fleet_adapter.py) under the `rmf_demos` repo. The C++ API for **Traffic Light Control** fleets (i.e. fleets that only allow RMF to pause/resume each mobile robot) can also be found in the `rmf_fleet_adapter` package of the `rmf_ros2` repo. The API reuses the `Adapter` class and requires users to initialize their fleet using either of the APIs [here](https://github.com/open-rmf/rmf_ros2/blob/9b4b8a8cc38b323f875a55c70f307446584d1639/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L106-L180). The user has the option to integrate via the [`TrafficLight`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/TrafficLight.hpp) API or for greater convenience, via the [`EasyTrafficLight`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyTrafficLight.hpp) API. diff --git a/src/integration_fleets_tutorial.md b/src/integration_fleets_tutorial.md index 295a5da..c7a432b 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -88,7 +88,7 @@ rmf_fleet: delivery: False clean: False finishing_request: "nothing" # [park, charge, nothing] - action: [] # Optional, list of performable action names + action: [] # Optional, list of custom actions that the fleet can perform # DeliveryBot CONFIG ================================================================= @@ -336,3 +336,337 @@ Parts of the [`RobotAPI`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_de ``` Further parameters may be added to `RobotAPI` to be used in these callbacks if required, such as authentication details and task IDs. You may also wish to write additional methods in `RobotAPI` and call them in your `fleet_adapter.py` for specific use cases. + +## 5. Create your fleet adapter! + +Now that we have our components ready, we can start writing our fleet adapter. You may either + +- Use the Full Control Python fleet adapter as a starting point and optionally add customization to its logic by modifying the RobotCommandHandle code, or +- Use our out-of-the-box fleet adapter logic with Easy Full Control with the [`easy_fleet_adapter`]() template provided in `fleet_adapter_template`. + +The following steps elaborate on how an **Easy Full Control** adapter can be written. It uses the same C++ API as the Full Control fleet adapter, with an additional layer of the [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class that helps to set up the fleet adapter's internal logic without user intervention. You will only need to parse your `config.yaml` and navigation graphs, as well as have some required callback functions ready, to create and start using your fleet adapter. + +Some code snippets from the [`easy_fleet_adapter.py`](https://github.com/open-rmf/rmf_demos/blob/feature/easy_full_control/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/easy_fleet_adapter.py) implemented in [rmf_demos_fleet_adapter](https://github.com/open-rmf/rmf_demos/tree/main/rmf_demos_fleet_adapter) are provided as examples. + +### 5a. Create a `FleetAdapter` class + +For ease of implementation, we create a `FleetAdapter` class to keep track of some fleet parameters and to spin up our adapter node easily. + +```python +class FleetAdapter: + + def __init__(self, config_path, nav_graph_path, node, use_sim_time): + # Load config yaml + with open(config_path, "r") as f: + config_yaml = yaml.safe_load(f) + # Initialize robot API for this fleet + fleet_config = config_yaml['rmf_fleet'] + prefix = 'http://' + fleet_config['fleet_manager']['ip'] + \ + ':' + str(fleet_config['fleet_manager']['port']) + self.api = RobotAPI( + prefix, + fleet_config['fleet_manager']['user'], + fleet_config['fleet_manager']['password']) + + node.declare_parameter('server_uri', rclpy.Parameter.Type.STRING) + server_uri = node.get_parameter( + 'server_uri').get_parameter_value().string_value + if server_uri == "": + server_uri = None +``` + +Here we load the fleet configuration and navigation graph YAML files, and set up access to the `RobotAPI` class that we updated previously. The `server_uri` parameter is also initialized here to be used with RMF Web. + +```python + # Create EasyFullControl adapter + self.configuration = adpt.easy_full_control.Configuration.make( + config_path, nav_graph_path, server_uri) + self.adapter = self.initialize_fleet( + self.configuration, config_yaml['robots'], node, use_sim_time) +``` + +We then create the `Configuration` object that will be required for the instantiation of the fleet adapter. This object encapsulates all the information about your fleet configuration and navigation graph and passes them on easily to the fleet adapter. + +Now we are ready to make the Easy Full Control adapter, which will be done in the `initialize_fleet` method. + +### 5b. Make the adapter + +```python + def initialize_fleet(self, configuration, robots_yaml, node, use_sim_time): + # Make the easy full control + easy_full_control = adpt.EasyFullControl.make(configuration) + + if use_sim_time: + easy_full_control.node.use_sim_time() +``` + +Inside the `initialize_fleet` method, we create the fleet adapter by passing our `Configuration` object to `EasyFullControl`'s `make(~)` function. + +### 5c. Create individual robot callbacks + +Before we can add our robots to the fleet, we need to define callbacks for each of them to perform various functions. These callbacks are needed for RMF to +- Retrieve the robot's state information, such as charger name, current location, battery state of charge, etc. (`GetStateCallback`) +- Send navigation (`NavigationRequest`), docking (`DockRequest`) or action (`ActionExecutor`) commands to the robot +- Request for the robot to stop moving (`StopRequest`) + +They are defined in the [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class, take a moment to look at the functions and the arguments they should take before implementing your own. + +If you have multiple robots to be added to your fleet that use the same callbacks, you can opt to create a generic function that takes in an identification argument (e.g. robot name) to avoid re-writing similar code. Some examples from `rmf_demos_fleet_adapter` below show how they can be implemented. Here we use parameters like `self.cmd_id` and `self.actions` to keep track of tasks and processes being carried out for the demos integration. + +**RobotState** + +The `GetStateCallback` function will be called at every update interval for RMF to keep tabs on the robot's whereabouts and availability for task allocation. It returns a [`RobotState`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp#L230) object or `None` if the robot state cannot be queried. + +```python + def _robot_state(robot_name): + # Use the RobotAPI to retrieve the robot's state information + data = self.api.data(robot_name) + + # If there is no information available, return None + if data is None or data['success'] is False: + return None + + # Return the robot information in a RobotState object + pos = data['data']['position'] + action = False + if robot_name in self.actions and \ + self.actions[robot_name] is not None: + action = True + state = adpt.easy_full_control.RobotState( + robot, + robot_config['charger']['waypoint'], + data['data']['map_name'], + [pos['x'], pos['y'], pos['yaw']], + data['data']['battery'], + action) + self.last_map[robot_name] = data['data']['map_name'] + return state +``` + +**Navigation** + +The `NavigationRequest` callback requires users to trigger a robot-specific navigation command, and call `execution.finished()` when the robot has reached its target waypoint. + +```python + def _navigate(robot_name, map_name, goal, execution): + # Clear current ongoing navigations if any + if robot_name in self.nav_threads: + if self.nav_threads[robot_name] is not None: + if self.nav_threads[robot_name].is_alive(): + self.nav_threads[robot_name].join() + + cmd_id = self.next_id + self.next_id += 1 + self.cmd_ids[robot_name] = cmd_id + + # Use RobotAPI to send navigation commands + self.api.navigate(robot_name, cmd_id, goal, map_name) + + # Internal tracking that calls execution.finished() + # when the robot reaches the destination + with self._lock: + self.nav_threads[robot_name] = None + self.nav_threads[robot_name] = threading.Thread( + target=self.start_navigation, + args=(robot_name, execution)) + self.nav_threads[robot_name].start() +``` + +In `rmf_demos_fleet_adapter`, this is done by passing the execution object to an internal function `start_navigation()` that continually checks the robot's navigation progress using RobotAPI's `navigate()` method. You can also use `execution.update_request()` to ask RMF to replan its trajectory and update the remaining duration for the navigation task. + +```python + def start_navigation(self, + robot_name, + execution): + while not self.api.process_completed(robot_name, + self.cmd_ids[robot_name]): + remaining_time = self.api.navigation_remaining_duration( + robot_name, self.cmd_ids[robot_name]) + if remaining_time: + remaining_time = datetime.timedelta(seconds=remaining_time) + request_replan = self.api.requires_replan(robot_name) + execution.update_request(request_replan, remaining_time) + # Navigation completed + execution.finished() +``` + +**DockRequest** + +Similarly, the `DockRequest` callback should call RobotAPI's `dock()` method and keep track of the robot's docking progress, and trigger `execution.finished()` when it is done with the task. + +```python + def _dock(robot_name, dock_name, execution): + if dock_name not in self.docks: + node.get_logger().info( + f'Requested dock {dock_name} not found, ' + f'ignoring docking request') + return + + # Clear current ongoing docking if any + if robot_name in self.traj_threads: + if self.traj_threads[robot_name] is not None: + if self.traj_threads[robot_name].is_alive(): + self.traj_threads[robot_name].join() + + cmd_id = self.next_id + self.next_id += 1 + self.cmd_ids[robot_name] = cmd_id + self.api.start_process( + robot_name, cmd_id, dock_name, self.last_map[robot_name]) + + positions = [] + for wp in self.docks[dock_name]: + positions.append([wp.x, wp.y, wp.yaw]) + task_completed_cb = self.api.navigation_completed + node.get_logger().info( + f"Robot {robot_name} is docking at {dock_name}...") + + with self._lock: + self.traj_threads[robot_name] = None + self.traj_threads[robot_name] = threading.Thread( + target=self.start_trajectory, + args=(task_completed_cb, robot_name, positions, + execution.handle(), execution)) + self.traj_threads[robot_name].start() +``` + +In this example, we have an internal function `start_trajectory()` to check on the docking process. We also made use of RMF adapter's scheduling tools to create trajectories for this request, allowing for additional monitoring of the robot. + +```python + # Track trajectory of docking or custom actions + def start_trajectory(self, + task_completed_cb, + robot_name, + positions, + update_handle, + execution): + while not task_completed_cb(robot_name, self.cmd_ids[robot_name]): + now = datetime.datetime.fromtimestamp(0) + \ + self.adapter.node.now() + traj = schedule.make_trajectory( + self.configuration.vehicle_traits(), + now, + positions) + itinerary = schedule.Route(self.last_map[robot_name], traj) + if update_handle is not None: + participant = update_handle.get_unstable_participant() + participant.set_itinerary([itinerary]) + execution.finished() +``` + +**Custom Actions** + +If you added custom actions to your `config.yaml`, you can specify the logic and trigger for the action using the `ActionExecutor` callback. As usual, you will have to call `execution.finished()` to signal the end of the performed action. The action name that you added will be carried here under `category`. This example stores the `ActionExecution` object in `self.actions`, and then uses an additional `toggle_action()` API to relay to the fleet manager that it can start on the given action. + +```python + def _action_executor( + robot_name: str, + category: str, + description: dict, + execution: adpt.robot_update_handle.ActionExecution): + self.actions[robot_name] = execution + self.api.toggle_action(robot_name, True) +``` + +The sections [User-defined Task](./task_userdefined.md) and [Supporting a New Task](./task_new.md) of this Book explain in further detail how an `ActionExecutor` function can be written and how a custom task can be created respectively. + +**Stop** + +`StopRequest` simply calls RobotAPI's corresponding method to request for the robot to stop moving. + +```python + def _stop(robot_name): + cmd_id = self.next_id + self.next_id += 1 + self.cmd_ids[robot_name] = cmd_id + return self.api.stop(robot_name, cmd_id) +``` + +### 5d. Add robot to the fleet adapter + +To add robots to the fleet adapter, you will need to pass the robot's starting state and the callbacks defined above to `EasyFullControl`'s `add_robot(~)` method. + +```python + # Add the robots + for robot in robots_yaml: + node.get_logger().info(f'Found robot {robot}') + success = False + while success is False: + state = _robot_state(robot) + if state is None: + time.sleep(0.2) + continue + success = True + # Add robot to fleet + easy_full_control.add_robot( + state, + partial(_robot_state, robot), + partial(_navigate, robot), + partial(_stop, robot), + partial(_dock, robot), + partial(_action_executor, robot)) + + return easy_full_control +``` + +### 5e. Initialize the `FleetAdapter` + +With the `FleetAdapter` set up, we can create our adapter instance. We take in the configuration and navigation graph file paths as command line arguments and pass them to the `FleetAdapter` class along with a ROS 2 node for the command handle. Additionally you might want to use the `use_sim_time` parameter if you would like to run the adapter in simulation. + +```python +def main(argv=sys.argv): + # Init rclpy and adapter + rclpy.init(args=argv) + adpt.init_rclcpp() + args_without_ros = rclpy.utilities.remove_ros_args(argv) + + parser = argparse.ArgumentParser( + prog="fleet_adapter", + description="Configure and spin up the fleet adapter") + parser.add_argument("-c", "--config_file", type=str, required=True, + help="Path to the config.yaml file") + parser.add_argument("-n", "--nav_graph", type=str, required=True, + help="Path to the nav_graph for this fleet adapter") + parser.add_argument("-sim", "--use_sim_time", action="store_true", + help='Use sim time, default: false') + args = parser.parse_args(args_without_ros[1:]) + print(f"Starting fleet adapter...") + + config_path = args.config_file + nav_graph_path = args.nav_graph + + # Load config and nav graph yamls + with open(config_path, "r") as f: + config_yaml = yaml.safe_load(f) + + # ROS 2 node for the command handle + fleet_name = config_yaml['rmf_fleet']['name'] + node = rclpy.node.Node(f'{fleet_name}_command_handle') + + # Enable sim time for testing offline + if args.use_sim_time: + param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + node.set_parameters([param]) + + adapter = FleetAdapter( + config_path, + nav_graph_path, + node, + args.use_sim_time) + + # Create executor for the command handle node + rclpy_executor = rclpy.executors.SingleThreadedExecutor() + rclpy_executor.add_node(node) + + # Start the fleet adapter + rclpy_executor.spin() + + # Shutdown + node.destroy_node() + rclpy_executor.shutdown() + rclpy.shutdown() + + +if __name__ == '__main__': + main(sys.argv) +``` \ No newline at end of file From 28a17aa5c57d809dc9c442822a583fe4fe6e8c1e Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Mon, 22 May 2023 17:31:32 +0800 Subject: [PATCH 4/9] Remove max_delay Signed-off-by: Xi Yu Oh --- src/integration_fleets_tutorial.md | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/integration_fleets_tutorial.md b/src/integration_fleets_tutorial.md index c7a432b..39b9073 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -80,7 +80,6 @@ rmf_fleet: power: 760.0 # W recharge_threshold: 0.20 # Battery level below which robots in this fleet will not operate recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks - max_delay: 10.0 # Allowed seconds of delay of the current itinerary before it gets interrupted and replanned publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency account_for_battery_drain: True task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing @@ -97,8 +96,6 @@ robots: # fleet. # Configuration for first robot in this fleet deliverybot1: - robot_config: - max_delay: 10.0 # Allowed seconds of delay of the current itinerary before it gets interrupted and replanned rmf_config: robot_state_update_frequency: 0.5 start: @@ -112,8 +109,6 @@ robots: # Configuration for the second robot in this fleet if there is a second robot # Uncomment if more than one robot exists. # deliverybot2: - # robot_config: - # max_delay: 10.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned # rmf_config: # robot_state_update_frequency: 0.5 # start: @@ -156,8 +151,6 @@ reference_coordinates: - `recharge_soc`: The fraction of total battery capacity to which the robot should be charged. - - `max_delay`: Allowed seconds of delay of the current itinerary before it gets interrupted and replanned. Note: currently used in `easy_fleet_adapter.py`. - - `task_capabilities`: The tasks that the robot can perform between `loop`, `delivery` and `clean`. - `finishing_request`: What the robot should do when it finishes its task, can be set to `park`, `charge` or `nothing`. @@ -168,8 +161,6 @@ reference_coordinates: - `deliveryBot1`: Name of the robot. - - `max_delay`: Allowed seconds before interruption occurs and replanning happens. Note: currently used in `fleet_adapter.py`, to be moved to the `rmf_fleet` section. - - `robot_state_update_frequency`: How frequently should the robot update the fleet. - `start`: Specifies the starting map name, initial waypoint (x, y) and orientation (in radians) of the robot. From 869825504668910b284135ce5b4c0cac206868b6 Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Mon, 22 May 2023 18:27:59 +0800 Subject: [PATCH 5/9] Update add robot loop Signed-off-by: Xi Yu Oh --- src/integration_fleets_tutorial.md | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/integration_fleets_tutorial.md b/src/integration_fleets_tutorial.md index 39b9073..948d427 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -575,20 +575,21 @@ The sections [User-defined Task](./task_userdefined.md) and [Supporting a New Ta ### 5d. Add robot to the fleet adapter -To add robots to the fleet adapter, you will need to pass the robot's starting state and the callbacks defined above to `EasyFullControl`'s `add_robot(~)` method. +To add robots to the fleet adapter, you will need to pass the robot's starting state and the callbacks defined above to `EasyFullControl`'s `add_robot(~)` method. Here we keep adding the robots listed in our `config.yaml` until we found all of them. ```python # Add the robots - for robot in robots_yaml: - node.get_logger().info(f'Found robot {robot}') - success = False - while success is False: + missing_robots = robots_yaml + while len(missing_robots) > 0: + for robot in list(missing_robots.keys()): + node.get_logger().info(f'Connecting to robot [{robot}]') + robot_config = robots_yaml[robot]['rmf_config'] state = _robot_state(robot) if state is None: + node.get_logger().info(f'Unable to find robot [{robot}], trying again...') time.sleep(0.2) continue - success = True - # Add robot to fleet + # Found robot, add to fleet easy_full_control.add_robot( state, partial(_robot_state, robot), @@ -596,6 +597,8 @@ To add robots to the fleet adapter, you will need to pass the robot's starting s partial(_stop, robot), partial(_dock, robot), partial(_action_executor, robot)) + node.get_logger().info(f'Successfully added new robot: [{robot}]') + del missing_robots[robot] return easy_full_control ``` From 78f1f6bb57f949baa77fc69f464bf26c2a2e745d Mon Sep 17 00:00:00 2001 From: Xi Yu Oh Date: Mon, 22 May 2023 18:35:48 +0800 Subject: [PATCH 6/9] Update link Signed-off-by: Xi Yu Oh --- src/integration_fleets_tutorial.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/integration_fleets_tutorial.md b/src/integration_fleets_tutorial.md index 948d427..a5194f7 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -333,7 +333,7 @@ Further parameters may be added to `RobotAPI` to be used in these callbacks if r Now that we have our components ready, we can start writing our fleet adapter. You may either - Use the Full Control Python fleet adapter as a starting point and optionally add customization to its logic by modifying the RobotCommandHandle code, or -- Use our out-of-the-box fleet adapter logic with Easy Full Control with the [`easy_fleet_adapter`]() template provided in `fleet_adapter_template`. +- Use our out-of-the-box fleet adapter logic with Easy Full Control with the [`easy_fleet_adapter`](https://github.com/open-rmf/fleet_adapter_template/blob/xiyu/update_EFC/fleet_adapter_template/fleet_adapter_template/easy_fleet_adapter.py) template provided in `fleet_adapter_template`. The following steps elaborate on how an **Easy Full Control** adapter can be written. It uses the same C++ API as the Full Control fleet adapter, with an additional layer of the [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class that helps to set up the fleet adapter's internal logic without user intervention. You will only need to parse your `config.yaml` and navigation graphs, as well as have some required callback functions ready, to create and start using your fleet adapter. From d1d762b471d154ef86454db45d15a947e2acdbf5 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Mon, 2 Oct 2023 22:29:49 +0800 Subject: [PATCH 7/9] Easy API and PerformAction tutorials Signed-off-by: Xiyu Oh --- src/SUMMARY.md | 3 +- src/integration_fleets_action_tutorial.md | 112 ++++ src/integration_fleets_adapter_tutorial.md | 463 ++++++++++++++ src/integration_fleets_tutorial.md | 666 --------------------- 4 files changed, 577 insertions(+), 667 deletions(-) create mode 100644 src/integration_fleets_action_tutorial.md create mode 100644 src/integration_fleets_adapter_tutorial.md delete mode 100644 src/integration_fleets_tutorial.md diff --git a/src/SUMMARY.md b/src/SUMMARY.md index 31dd9ba..ac2415f 100644 --- a/src/SUMMARY.md +++ b/src/SUMMARY.md @@ -21,7 +21,8 @@ - [Integration](./integration.md) - [Navigation Maps](./integration_nav-maps.md) - [Mobile Robot Fleets](./integration_fleets.md) - - [Tutorials](./integration_fleets_tutorial.md) + - [Fleet Adapter Tutorial](./integration_fleets_adapter_tutorial.md) + - [PerformAction Tutorial](./integration_fleets_action_tutorial.md) - [Free Fleet](./integration_free-fleet.md) - [Read-Only Fleets](./integration_read-only.md) - [Doors](./integration_doors.md) diff --git a/src/integration_fleets_action_tutorial.md b/src/integration_fleets_action_tutorial.md new file mode 100644 index 0000000..456cf8e --- /dev/null +++ b/src/integration_fleets_action_tutorial.md @@ -0,0 +1,112 @@ +# PerformAction Tutorial (Python) + +This tutorial is an extension of the Fleet Adapter Tutorial and will guide you to write custom actions in your fleet adapter. While RMF offers a few standard tasks, we understand that different robots may be equipped and programmed to perform different types of actions, such as cleaning, object-picking, teleoperation, and so on. By supporting custom tasks, users can trigger a custom action specified in the fleet adapter's `config.yaml` beforehand, and RMF would relinquish control of the robot until it is signalled that the robot has completed the custom action. You may explore the [Supporting a new Task in RMF](./task_new.md) section to read more about supporting custom tasks and how you can create your own task JSON to be sent to RMF. + +In this tutorial, we will refer to a simplified version of the `rmf_demos_fleet_adapter` to implement a `Clean` PerformAction capability in our fleet adapter. + +## 1. Define the PerformAction in the fleet `config.yaml` + +We will need to define the name of the action in the fleet configuration, so that RMF recognizes this action as performable when a task is submitted and is able to dispatch it to a fleet that can fulfil it. In our `config.yaml` under the `rmf_fleet` section, we can provide a list of performable actions for our fleet. For example, let's define `clean` as an action supported by this fleet: + +```yaml +rmf_fleet: + actions: ["clean"] +``` + +## 2. Apply action execution logic inside our fleet adapter + +After RMF receives a task consisting of this action and dispatches it to the right fleet, the fleet adapter's `execute_action(~)` callback will be triggered. The `category` parsed to this callback corresponds to the action name that we have previously defined, and the `description` consists of any details about the action that we might be interested in. + +Assume that this is the task JSON submitted to RMF: +```json +{ + "type": "dispatch_task_request", + "request": { + "unix_millis_earliest_start_time": start_time, + "category": "clean", + "description": { + "zone": "clean_lobby" + } + } +} +``` + +In our example, the `category` provided would be `clean`, and the `description` would contain which cleaning zone this task is directing our robot to, which is `clean_lobby`. Hence, we will need to implement the logic in our `execute_action(~)`: + +```python + def execute_action(self, category: str, description: dict, execution): + self.execution = execution + + if category == 'clean': + self.perform_clean(description['zone']) + + def perform_clean(self, zone): + if self.api.start_activity(self.name, 'clean', zone): + self.node.get_logger().info( + f'Commanding [{self.name}] to clean zone [{zone}]' + ) + else: + self.node.get_logger().error( + f'Fleet manager for [{self.name}] does not know how to ' + f'clean zone [{zone}]. We will terminate the activity.' + ) + self.execution.finished() + self.execution = None +``` + +Since our fleet may be capable of performing multiple custom actions, we will need to conduct a check to ensure that the `category` received matches the robot API that we are targeting. Upon receiving a `clean` action, we can trigger the robot's API accordingly. + +## 3. Implement the robot API for the custom action + +This is where the `start_activity(~)` method inside `RobotClientAPI.py` comes into play. We would require it to implement the API call to the robot to start the cleaning activity. As an example, if the robot API uses REST to make calls to the robot, the implemented method may look like this: + +```python + def start_activity( + self, + robot_name: str, + activity: str, + label: str + ): + ''' Request the robot to begin a process. This is specific to the robot + and the use case. For example, load/unload a cart for Deliverybot + or begin cleaning a zone for a cleaning robot.''' + url = ( + self.prefix + + f"/open-rmf/rmf_demos_fm/start_activity?robot_name={robot_name}" + ) + # data fields: task, map_name, destination{}, data{} + data = {'activity': activity, 'label': label} + try: + response = requests.post(url, timeout=self.timeout, json=data) + response.raise_for_status() + if self.debug: + print(f'Response: {response.json()}') + + if response.json()['success']: + return True + + # If we get a response with success=False, then + return False + except HTTPError as http_err: + print(f'HTTP error for {robot_name} in start_activity: {http_err}') + except Exception as err: + print(f'Other error {robot_name} in start_activity: {err}') + return False +``` + +## 4. Complete the action + +Since we stored a `self.execution` object in our `RobotAdapter`, we will be notified when any execution (navigation, stop, or action) is completed as the update loop continually calls `is_command_completed` to check on its status. + +```python + def update(self, state): + activity_identifier = None + if self.execution: + if self.api.is_command_completed(): + self.execution.finished() + self.execution = None + else: + activity_identifier = self.execution.identifier +``` + +If your implementation requires a separate callback to mark the execution as finished, you can create a new function to conduct this check and call `self.execution.finished()` when the action is completed. diff --git a/src/integration_fleets_adapter_tutorial.md b/src/integration_fleets_adapter_tutorial.md new file mode 100644 index 0000000..d286f1f --- /dev/null +++ b/src/integration_fleets_adapter_tutorial.md @@ -0,0 +1,463 @@ +# Fleet Adapter Tutorial (Python) + +`fleet_adapter` acts as a bridge between the robots and the core RMF system. + +Its responsibilities include but are not limited to: + +- Updating the traffic schedule with the fleet robot's positions + +- Responding to tasks + +- Controlling the vendor robots. + +The `fleet_adapter` receives information (position, current ongoing tasks, battery levels etc.) about each robot in the fleet and sends them to the core RMF system for task planning and scheduling. + +- When the core RMF system has a task to dispatch, it communicates with the various fleet adapters to check which fleet is suitable for taking this task. + +- It sends a request, to which fleet adapters respond by submitting their fleet robots' availability and statuses. + +- RMF determines the best fleet for the task and responds to the winning bid, i.e. the fleet that is selected. The response contains navigation commands relevant to the delegated task. + +- The fleet adapter will then send the navigation commands to the robot in appropriate API. + +> The tutorial provided below is based on the [rmf_demos_fleet_adapter](https://github.com/open-rmf/rmf_demos/tree/main/rmf_demos_fleet_adapter) implemented in the [rmf_demos](https://github.com/open-rmf/rmf_demos) repository. This specific implementation is written in Python and uses REST API as an interface between the fleet adapter and fleet manager. You may choose to use other APIs for your own integration. + +## 1. Pre-requisites + +### Fetch dependencies + +Since the `rmf_demos_fleet_adapter` uses REST API as an interface between the fleet adapter and robot fleet manager, we will need to install the required dependencies to use FastAPI. +```bash +pip3 install fastapi uvicorn +``` + +### Get started with the fleet adapter template + +Create a workspace and clone the [fleet_adapter_template](https://github.com/open-rmf/fleet_adapter_template) repository. + +```bash +mkdir -p ~/rmf_ws/src +cd ~/rmf_ws/src/ +git clone https://github.com/open-rmf/fleet_adapter_template.git +``` + +This template contains the code for both Full Control and Easy Full Control fleet adapters. Both implementations use API calls in [`RobotClientAPI.py`](https://github.com/open-rmf/fleet_adapter_template/blob/main/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py) to communicate with the robots. + +## 2. Update the `config.yaml` file + +The `config.yaml` file contains important parameters for setting up the fleet adapter. Users should start by updating these configurations describing their fleet robots. + +It is important to stick to the provided fields in the sample `config.yaml` below, otherwise there will be import errors when parsing this YAML file to the fleet adapter. If you would like to edit any of the field names or value range, or even append additional fields, please ensure that you also modify the part of your fleet adapter code that handles this configuration import accordingly. + +Some fields are optional as indicated below. + +```yaml +# FLEET CONFIG ================================================================= +# RMF Fleet parameters + +rmf_fleet: + name: "tinyRobot" + limits: + linear: [0.5, 0.75] # velocity, acceleration + angular: [0.6, 2.0] # velocity, acceleration + profile: # Robot profile is modelled as a circle + footprint: 0.3 # radius in m + vicinity: 0.5 # radius in m + reversible: True # whether robots in this fleet can reverse + battery_system: + voltage: 12.0 # V + capacity: 24.0 # Ahr + charging_current: 5.0 # A + mechanical_system: + mass: 20.0 # kg + moment_of_inertia: 10.0 #kgm^2 + friction_coefficient: 0.22 + ambient_system: + power: 20.0 # W + tool_system: + power: 0.0 # W + recharge_threshold: 0.10 # Battery level below which robots in this fleet will not operate + recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks + publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency + 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: True + actions: ["teleop"] + finishing_request: "park" # [park, charge, nothing] + responsive_wait: True # Should responsive wait be on/off for the whole fleet by default? False if not specified. + robots: + tinyRobot1: + charger: "tinyRobot1_charger" + responsive_wait: False # Should responsive wait be on/off for this specific robot? Overrides the fleet-wide setting. + tinyRobot2: + charger: "tinyRobot2_charger" + # No mention of responsive_wait means the fleet-wide setting will be used + + robot_state_update_frequency: 10.0 # Hz + +fleet_manager: + prefix: "http://127.0.0.1:8080" + user: "some_user" + password: "some_password" + +# TRANSFORM CONFIG ============================================================= +# For computing transforms between Robot and RMF coordinate systems + +# Optional +reference_coordinates: + L1: + rmf: [[20.33, -3.156], + [8.908, -2.57], + [13.02, -3.601], + [21.93, -4.124]] + robot: [[59, 399], + [57, 172], + [68, 251], + [75, 429]] +``` + +- `rmf_fleet`: Important fleet parameters including vehicle traits, task capabilities and user information for connecting to the fleet manager. + + - `limits`: Maximum values for linear and angular accelerations and velocities. + + - `profile`: Radius of the footprint and personal vicinity of the vehicles in this fleet. + + - `reversible`: A flag to enable/disable reverse traversal in the robot. + + - `battery_system`: Information about the battery's voltage, capacity and charging current. + + - `recharge_threshold`: Sets a value for minimum charge below which the robot must return to its charger. + + - `recharge_soc`: The fraction of total battery capacity to which the robot should be charged. + + - `task_capabilities`: The tasks that the robot can perform between `loop`, `delivery` and `clean`. + + - `account_for_battery_drain`: Whether RMF should consider the battery drain of the robots before dispatching tasks. + + - `action` [Optional]: A list of custom performable actions for the fleet. + + - `finishing_request`: What the robot should do when it finishes its task, can be set to `park`, `charge` or `nothing`. + + - `responsive_wait` [Optional]: True # Should responsive wait be on/off for the whole fleet by default? False if not specified. + + - `robots`: Information about each individual robot in the fleet. Each item in this section corresponds to the configuration for a single robot in the fleet. You may add more robots accordingly. + + - `tinyRobot1`: Name of the robot. + + - `charger`: Name of the robot's charging point. + + - `responsive_wait`: Whether this specific robot should turn its responsive wait on/off. Overrides the fleet-wide setting. + + - `robot_state_update_frequency`: How frequently should the robots update the fleet. + +- `fleet_manager`: The *prefix*, *user* and *password* fields that can be configured to suit your chosen API. Do make sure to also edit the corresponding fields in `RobotClientAPI.py` if you do modify them. These parameters will be used to set up connection with your fleet manager/robots. + +- `reference_coordinates` [Optional]: If the fleet robots are not operating in the same coordinate system as RMF, you can provide two sets of (x, y) coordinates that correspond to the same locations in each system. This helps with estimating coordinate transformations from one frame to another. A minimum of 4 matching waypoints is recommended. + + Note: this is not being implemented in `rmf_demos_fleet_adapter` as the demos robots and RMF are using the same coordinate system. + +## 3. Create navigation graphs + +A navigation graph is required to be parsed to the fleet adapter so that RMF can understand the robots' environment. They can be created using the [RMF Traffic Editor](https://github.com/open-rmf/rmf_traffic_editor.git) and the [`building_map_generator nav`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp) CLI provided. Refer to the traffic editor repo's README for installation and map generation instructions. + +You may also want to look through the [Traffic Editor](./traffic-editor.md) section of this Book for detailed information and instructions on creating your own digital maps. + +You should now have a YAML file with information about the lanes and waypoints (among other information) that describe the paths your robot fleet can take. + + +## 4. Fill in your `RobotAPI` + +[`RobotClientAPI.py`](https://github.com/open-rmf/fleet_adapter_template/blob/main/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py) provides a set of methods being used by the fleet adapter. These callbacks are triggered when RMF needs to send or retrieve information via the fleet adapter to/from the managed robots. To cater to the interface of your choice, you need to fill in the missing code blocks marked with `# IMPLEMENT YOUR CODE HERE #` within `RobotAPI` with logics to send or retrieve the corresponding information. For example, if your robot uses REST API to interface with the fleet adapter, you will need to make HTTP request calls to the appropriate endpoints within these functions. + +You may refer to the [`RobotAPI`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py) class implementated for `rmf_demos_fleet_adapter` for examples of how these methods can be filled up. + +- `navigate`: Sends a navigation command to the robot API. It takes in the destination coordinates from RMF, desired map name and optional speed limit. +- `start_activity`: Sends a command to the robot to start performing a task. This method is helpful for custom performable actions that are triggered by `execute_action()`. +- `stop`: Commands the robot to stop moving. +- `position`, `map` and `battery_soc`: Retrieves the robot's current position in its coordinate frame in the format `[x, y, theta]`, its current map name, and its battery state of charge. In `rmf_demos_fleet_adapter` these methods are consolidated under `get_data()`. +- `is_command_completed`: Checks if the robot has completed the ongoing process or task. In `rmf_demos_fleet_adapter`, this is implemented under the `RobotUpdateData` class. Depending on your robot API you may choose to integrate it either way. This callback will help RMF recognize when a dispatched command is completed, and proceed to send subsequent commands. + +Further parameters may be added to `RobotAPI` to be used in these callbacks if required, such as authentication details and task IDs. You may also wish to write additional methods in either `RobotAPI` and `fleet_adapter.py` for specific use cases. The `rmf_demos_fleet_adapter` implementation demonstrates this for a `Teleoperation` action, which will be elaborated more in the [PerformAction tutorial](./integration_fleets_action_tutorial.md). + +## 5. Create your fleet adapter! + +Now that we have our components ready, we can start creating our fleet adapter. `fleet_adapter.py` uses the Easy Full Control API to easily create an `Adapter` instance and set up the fleet configurations and robots by parsing the configuration YAML file that we have prepared previously. Since we have defined our `RobotAPI`, the methods implemented will be used by the callbacks in `fleet_adapter.py` so that RMF can retrieve robot information and send out navigation or action commands appropriately. + +You may wish to use the `fleet_adapter.py` available from the fleet adapter template and modify it according to what you'd like your fleet to achieve. + +## 6. Run your fleet adapter + +At this point, you should have 4 components ready in order to run your fleet adapter: +- `fleet_adapter.py` +- `RobotClientAPI.py` +- Fleet `config.yaml` file +- Navigation graph + +### a. Install ROS 2 and sRMF + +Before running your fleet adapter, make sure that you have ROS 2 and RMF installed by following the instructions [here](./installation.md). You have the option of installing the binaries or building from source for both. You may also wish to head over to our [RMF Github repo](https://github.com/open-rmf/rmf) for the latest updates and instructions for RMF installation. + +If you built ROS 2 and/or RMF from source, make sure to source the workspace that contains their built code before proceeding to the next step. + +### b. Build your fleet adapter package + +If you cloned the `fleet_adapter_template` repository, you would already have your Python scripts in a ROS 2 package. Otherwise, you can follow the instructions [here](https://docs.ros.org/en/rolling/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html) to create a package in your workspace. For the instructions below, we will use the package and module names used in the `fleet_adapter_template` package. + +With your scripts in the appropriate folder, go back to the root directory of your workspace and build the package. + +```bash +colcon build --packages-select fleet_adapter_template +``` + +### c. Run! + +We will now source our workspace and run the fleet adapter: + +```python +. ~/rmf_ws/install/setup.bash + +ros2 run fleet_adapter_template fleet_adapter -c -n +``` + +## 7. Deep dive into the code [Optional] + +The following steps elaborate on the Easy Full Control fleet adapter and what each part of the code does. + +### a. Import important parameters and create an Adapter + +When running our fleet adapter, we will need to parse in the fleet config file and navigation graphs that we created in earlier steps. These files will be passed to the EasyFullControl API to set up fleet configurations for the adapter. + +```python + config_path = args.config_file + nav_graph_path = args.nav_graph + + fleet_config = rmf_easy.FleetConfiguration.from_config_files( + config_path, nav_graph_path + ) + assert fleet_config, f'Failed to parse config file [{config_path}]' + + # Parse the yaml in Python to get the fleet_manager info + with open(config_path, "r") as f: + config_yaml = yaml.safe_load(f) +``` + +With these parameters, we can create an Adapter instance and add an EasyFullControl fleet to it. We would also want to configure the `use_sim_time` and `server_uri` parameters if the adapter should operate according to simulation clock or broadcast task updates to any websocket servers. + +```python + # ROS 2 node for the command handle + fleet_name = fleet_config.fleet_name + node = rclpy.node.Node(f'{fleet_name}_command_handle') + adapter = Adapter.make(f'{fleet_name}_fleet_adapter') + assert adapter, ( + 'Unable to initialize fleet adapter. ' + 'Please ensure RMF Schedule Node is running' + ) + + # Enable sim time for testing offline + if args.use_sim_time: + param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + node.set_parameters([param]) + adapter.node.use_sim_time() + + adapter.start() + time.sleep(1.0) + + if args.server_uri == '': + server_uri = None + else: + server_uri = args.server_uri + + fleet_config.server_uri = server_uri + fleet_handle = adapter.add_easy_fleet(fleet_config) +``` + +### b. Configure transformations between RMF and robot + +We have defined a helper function to compute the transforms between RMF and the robot's coordinates. In the event your robot operates in the same coordinates as RMF (e.g. in simulation), you won't need this portion of the code. + +```python +def compute_transforms(level, coords, node=None): + """Get transforms between RMF and robot coordinates.""" + rmf_coords = coords['rmf'] + robot_coords = coords['robot'] + tf = nudged.estimate(rmf_coords, robot_coords) + if node: + mse = nudged.estimate_error(tf, rmf_coords, robot_coords) + node.get_logger().info( + f"Transformation error estimate for {level}: {mse}" + ) + + return Transformation( + tf.get_rotation(), + tf.get_scale(), + tf.get_translation() + ) +``` + +Then, in our `main` function, we at the computed transforms to our fleet configuration. The EasyFullControl fleet adapter will process these transforms and send out navigation commands in the robot's coordinates accordingly. + +```python + # Configure the transforms between robot and RMF frames + for level, coords in config_yaml['reference_coordinates'].items(): + tf = compute_transforms(level, coords, node) + fleet_config.add_robot_coordinates_transformation(level, tf) +``` + +### c. Initialize the robot API and set up RobotAdapter + +The `config.yaml` should include any connection credentials we'd need to connect to our robot or robot fleet manager. We parse this to the `RobotAPI` to easily interact between RMF and the robot's API. + +```python + # Initialize robot API for this fleet + fleet_mgr_yaml = config_yaml['fleet_manager'] + api = RobotAPI(fleet_mgr_yaml) +``` + +Given a list of known robots from our `config.yaml`, we can initialize a `RobotAdapter` class for each robot that is supposed to be added to the fleet. + +```python + robots = {} + for robot_name in fleet_config.known_robots: + robot_config = fleet_config.get_known_robot_configuration(robot_name) + robots[robot_name] = RobotAdapter( + robot_name, robot_config, node, api, fleet_handle + ) +``` + +### d. Retrieve robot status and add robot to the fleet + +This update loop will allow us to update the `RobotUpdateHandle` with our robots' information asynchronously, such that any error in retrieving the status from one robot won't block the other robots from updating the fleet adapter. + +```python + update_period = 1.0/config_yaml['rmf_fleet'].get( + 'robot_state_update_frequency', 10.0 + ) + + def update_loop(): + asyncio.set_event_loop(asyncio.new_event_loop()) + while rclpy.ok(): + now = node.get_clock().now() + + # Update all the robots in parallel using a thread pool + update_jobs = [] + for robot in robots.keys(): + update_jobs.append(update_robot(robot)) + + asyncio.get_event_loop().run_until_complete( + asyncio.wait(update_jobs) + ) + + next_wakeup = now + Duration(nanoseconds=update_period*1e9) + while node.get_clock().now() < next_wakeup: + time.sleep(0.001) + + update_thread = threading.Thread(target=update_loop, args=()) + update_thread.start() +``` + +The function `update_robot()` is called to ensure that our robots' current map, position and battery state of charge will be updated properly. If the robot is new to the fleet handle, we will add it in via `add_robot()`. + +```python +@parallel +def update_robot(robot: RobotAdapter): + data = robot.api.get_data(robot.name) + if data is None: + return + + state = rmf_easy.RobotState( + data.map, + data.position, + data.battery_soc + ) + + if robot.update_handle is None: + robot.update_handle = robot.fleet_handle.add_robot( + robot.name, + state, + robot.configuration, + robot.make_callbacks() + ) + return + + robot.update(state) +``` + + +### e. Inside the `RobotAdapter` class + +The `RobotAdapter` class helps us to keep track of any ongoing process the robot may be carrying out, and perform the correct actions when RMFs sends a corresponding command. + +```python +class RobotAdapter: + def __init__( + self, + name: str, + configuration, + node, + api: RobotAPI, + fleet_handle + ): + self.name = name + self.execution = None + self.update_handle = None + self.configuration = configuration + self.node = node + self.api = api + self.fleet_handle = fleet_handle +``` + +There are 3 important callbacks that we need to pass on to the EasyFullControl API: + +- `navigate` +- `stop` +- `execute_action` + +As described above, each of these callbacks will be triggered by RMF when it needs to command to robot to do something. Hence, we define these callbacks in our `RobotAdapter`: + +```python + def navigate(self, destination, execution): + self.execution = execution + self.node.get_logger().info( + f'Commanding [{self.name}] to navigate to {destination.position} ' + f'on map [{destination.map}]' + ) + + self.api.navigate( + self.name, + destination.position, + destination.map, + destination.speed_limit + ) + + def stop(self, activity): + if self.execution is not None: + if self.execution.identifier.is_same(activity): + self.execution = None + self.stop(self.name) + + def execute_action(self, category: str, description: dict, execution): + ''' Trigger a custom action you would like your robot to perform. + You may wish to use RobotAPI.start_activity to trigger different + types of actions to your robot.''' + self.execution = execution + # ------------------------ # + # IMPLEMENT YOUR CODE HERE # + # ------------------------ # + return +``` + +Notice that `execute_action(~)` does not have any implemented code in the fleet adapter template. This callback is designed to be flexible and caters to custom performable actions that may not be availble under the tasks offered in RMF. You can learn how to design and compose your own actions and execute them from the fleet adapter in the [PerformAction tutorial](./integration_fleets_action_tutorial.md) section. + +```python + def make_callbacks(self): + return rmf_easy.RobotCallbacks( + lambda destination, execution: self.navigate( + destination, execution + ), + lambda activity: self.stop(activity), + lambda category, description, execution: self.execute_action( + category, description, execution + ) + ) +``` diff --git a/src/integration_fleets_tutorial.md b/src/integration_fleets_tutorial.md deleted file mode 100644 index a5194f7..0000000 --- a/src/integration_fleets_tutorial.md +++ /dev/null @@ -1,666 +0,0 @@ -# Fleet Adapter Tutorial (Python) - -`fleet_adapter` acts as a bridge between the robots and the core RMF system. - -Its responsibilities include but are not limited to: - -- Updating the traffic schedule with the fleet robot's positions - -- Responding to tasks - -- Controlling the vendor robots. - -The `fleet_adapter` receives information (position, current ongoing tasks, battery levels etc.) about each robot in the fleet and sends them to the core RMF system for task planning and scheduling. - -- When the core RMF system has a task to dispatch, it communicates with the various fleet adapters to check which fleet is suitable for taking this task. - -- It sends a request, to which fleet adapters respond by submitting their fleet robots' availability and statuses. - -- RMF determines the best fleet for the task and responds to the winning bid, i.e. the fleet that is selected. The response contains navigation commands relevant to the delegated task. - -- The fleet adapter will then send the navigation commands to the robot in appropriate API. - -> The tutorial provided below is based on the [rmf_demos_fleet_adapter](https://github.com/open-rmf/rmf_demos/tree/main/rmf_demos_fleet_adapter) implemented in the [rmf_demos](https://github.com/open-rmf/rmf_demos) repository. This specific implementation is written in Python and uses REST API as an interface between the fleet adapter and fleet manager. You may choose to use other APIs for your own integration. - -## 1. Pre-requisites - -### Fetch dependencies - -Since the `rmf_demos_fleet_adapter` uses REST API as an interface between the fleet adapter and robot fleet manager, we will need to install the required dependencies to use FastAPI. -```bash -pip3 install fastapi uvicorn -``` - -### Get started with the fleet adapter template - -Clone the [fleet_adapter_template](https://github.com/open-rmf/fleet_adapter_template) repository. - -```bash -git clone https://github.com/open-rmf/fleet_adapter_template.git -``` - -This template contains the code for both Full Control and Easy Full Control fleet adapters. Both implementations use API calls in [`RobotClientAPI.py`](https://github.com/open-rmf/fleet_adapter_template/blob/main/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py) to communicate with the robots. - -## 2. Update the `config.yaml` file - -The `config.yaml` file contains important parameters for setting up the fleet adapter. Users should start by updating these configurations describing their fleet robots. - -It is important to stick to the provided fields in the sample `config.yaml` below, otherwise there will be import errors when parsing this YAML file to the fleet adapter. If you would like to edit any of the field names or value range, or even append additional fields, please ensure that you also modify the part of your fleet adapter code that handles this configuration import accordingly. - -Some fields are optional as indicated below. - -```yaml -# FLEET CONFIG ================================================================= -# RMF Fleet parameters - -rmf_fleet: - name: "DeliveryBot" - fleet_manager: - prefix: "http://127.0.0.1:8000" - user: "some_user" - password: "some_password" - limits: - linear: [0.4, 0.2] # velocity, acceleration - angular: [0.3, 0.35] # velocity, acceleration - profile: # Robot profile is modelled as a circle - footprint: 0.5 # radius in m - vicinity: 0.6 # radius in m - reversible: False # whether robots in this fleet can reverse - battery_system: - voltage: 24.0 # V - capacity: 40.0 # Ahr - charging_current: 26.4 # A - mechanical_system: - mass: 80.0 # kg - moment_of_inertia: 20.0 #kgm^2 - friction_coefficient: 0.20 - ambient_system: - power: 20.0 # W - tool_system: - power: 760.0 # W - recharge_threshold: 0.20 # Battery level below which robots in this fleet will not operate - recharge_soc: 1.0 # Battery level to which robots in this fleet should be charged up to during recharging tasks - publish_fleet_state: 10.0 # Publish frequency for fleet state, ensure that it is same as robot_state_update_frequency - 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 - finishing_request: "nothing" # [park, charge, nothing] - action: [] # Optional, list of custom actions that the fleet can perform - -# DeliveryBot CONFIG ================================================================= - -robots: - # Here the user is expected to append the configuration for each robot in the - # fleet. - # Configuration for first robot in this fleet - deliverybot1: - rmf_config: - robot_state_update_frequency: 0.5 - start: - map_name: "L1" - # waypoint: "charger_deliverybot1" # Optional - # orientation: 0.0 # Optional, radians - waypoint: null - orientation: null - charger: - waypoint: "charger_deliverybot1" - # Configuration for the second robot in this fleet if there is a second robot - # Uncomment if more than one robot exists. - # deliverybot2: - # rmf_config: - # robot_state_update_frequency: 0.5 - # start: - # map_name: "L1" - # # waypoint: "charger_deliverybot2" # Optional - # # orientation: 0.0 # Optional, radians - # waypoint: null - # orientation: null - # charger: - # waypoint: "charger_deliverybot2" - -# TRANSFORM CONFIG ============================================================= -# For computing transforms between Robot and RMF coordinate systems - -# Optional -reference_coordinates: - rmf: [[20.33, -3.156], - [8.908, -2.57], - [13.02, -3.601], - [21.93, -4.124]] - robot: [[59, 399], - [57, 172], - [68, 251], - [75, 429]] -``` - -- `rmf_fleet`: Important fleet parameters including vehicle traits, task capabilities and user information for connecting to the fleet manager. - - - `fleet_manager`: The *prefix*, *user* and *password* fields that can be configured to suit your chosen API. Do make sure to also edit the corresponding fields in `fleet_adapter.py` if you do modify them. These parameters will be brought into `RobotClientAPI.py` for you to set up connection with your fleet manager/robots. - - - `limits`: Maximum values for linear and angular accelerations and velocities. - - - `profile`: Radius of the footprint and personal vicinity of the vehicles in this fleet. - - - `reversible`: A flag to enable/disable reverse traversal in the robot. - - - `battery_system`: Information about the battery's voltage, capacity and charging current. - - - `recharge_threshold`: Sets a value for minimum charge below which the robot must return to its charger. - - - `recharge_soc`: The fraction of total battery capacity to which the robot should be charged. - - - `task_capabilities`: The tasks that the robot can perform between `loop`, `delivery` and `clean`. - - - `finishing_request`: What the robot should do when it finishes its task, can be set to `park`, `charge` or `nothing`. - - - `action` [Optional]: A list of custom performable actions for the fleet. - -- `robots`: Information about each individual robot in the fleet. Each item in this section corresponds to the configuration for a single robot in the fleet. You may add more robots accordingly. - - - `deliveryBot1`: Name of the robot. - - - `robot_state_update_frequency`: How frequently should the robot update the fleet. - - - `start`: Specifies the starting map name, initial waypoint (x, y) and orientation (in radians) of the robot. - - - `charger waypoint`: Name of the robot's charging point. - -- `reference_coordinates` [Optional]: If the fleet robots are not operating in the same coordinate system as RMF, you can provide two sets of (x, y) coordinates that correspond to the same locations in each system. This helps with estimating coordinate transformations from one frame to another. A minimum of 4 matching waypoints is recommended. - - Note: this is not being implemented in `rmf_demos_fleet_adapter` as the demos robots and RMF are using the same coordinate system. - -## 3. Create navigation graphs - -A navigation graph is required to be parsed to the fleet adapter so that RMF can understand the robots' environment. They can be created using the [RMF Traffic Editor](https://github.com/open-rmf/rmf_traffic_editor.git) and the [`building_map_generator nav`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp) CLI provided. Refer to the traffic editor repo's README for installation and map generation instructions. - -You may also want to look through the [Traffic Editor](./traffic-editor.md) section of this Book for detailed information and instructions on creating your own digital maps. - -You should now have a YAML file with information about the lanes and waypoints (among other information) that describe the paths your robot fleet can take. - - -## 4. Fill in your `RobotAPI` - -[`RobotClientAPI.py`](https://github.com/open-rmf/fleet_adapter_template/blob/main/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py) provides a set of methods being used by the fleet adapter. These callbacks are triggered when RMF needs to send or retrieve information via the fleet adapter to/from the managed robots. To cater to the interface of your choice, you need to fill in the missing code blocks marked with `# IMPLEMENT YOUR CODE HERE #` within `RobotAPI` with logics to send or retrieve the corresponding information. For example, if your robot uses REST API to interface with the fleet adapter, you will need to make HTTP request calls to the appropriate endpoints within these functions. - -Parts of the [`RobotAPI`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py) class implementated for `rmf_demos_fleet_adapter` is further elaborated below to illustrate how you should fill in the missing code: - -- `position`: Retrieves the robot's current position in its coordinate frame in the format `[x, y, theta]` via a GET request. - -```python - def position(self, robot_name: str): - ''' Return [x, y, theta] expressed in the robot's coordinate frame or - None if any errors are encountered''' - if robot_name is not None: - url = self.prefix +\ - f'/open-rmf/rmf_demos_fm/status/?robot_name={robot_name}' - else: - url = self.prefix + f'/open-rmf/rmf_demos_fm/status' - try: - response = requests.get(url, self.timeout) - response.raise_for_status() - data = response.json() - if self.debug: - print(f'Response: {data}') - if not data['success']: - return None - x = data['data']['position']['x'] - y = data['data']['position']['y'] - angle = data['data']['position']['yaw'] - return [x, y, angle] - except HTTPError as http_err: - print(f'HTTP error: {http_err}') - except Exception as err: - print(f'Other error: {err}') - return None -``` - -- `navigate`: Sends a POST request to the robot's fleet manager with the destination coordinates. Returns true if the robot successfully accepts the request, else false. -```python - def navigate(self, - robot_name: str, - pose, - map_name: str, - speed_limit=0.0): - ''' Request the robot to navigate to pose:[x,y,theta] where x, y and - and theta are in the robot's coordinate convention. This function - should return True if the robot has accepted the request, - else False''' - assert(len(pose) > 2) - url = self.prefix +\ - f'/open-rmf/rmf_demos_fm/navigate/?robot_name={robot_name}' - data = {} # data fields: task, map_name, destination{}, data{} - data['map_name'] = map_name - data['destination'] = {'x': pose[0], 'y': pose[1], 'yaw': pose[2]} - data['speed_limit'] = speed_limit - try: - response = requests.post(url, timeout=self.timeout, json=data) - response.raise_for_status() - if self.debug: - print(f'Response: {response.json()}') - return response.json()['success'] - except HTTPError as http_err: - print(f'HTTP error: {http_err}') - except Exception as err: - print(f'Other error: {err}') - return False -``` - -- `start_process`: Sends a POST request to the robot's fleet manager asking it to perform a task. - -```python - def start_process(self, - robot_name: str, - process: str, - map_name: str): - ''' Request the robot to begin a process. This is specific to the robot - and the use case. For example, load/unload a cart for Deliverybot - or begin cleaning a zone for a cleaning robot. - Return True if the robot has accepted the request, else False''' - url = self.prefix +\ - f"/open-rmf/rmf_demos_fm/start_task?robot_name={robot_name}" - # data fields: task, map_name, destination{}, data{} - data = {'task': process, 'map_name': map_name} - try: - response = requests.post(url, timeout=self.timeout, json=data) - response.raise_for_status() - if self.debug: - print(f'Response: {response.json()}') - return response.json()['success'] - except HTTPError as http_err: - print(f'HTTP error: {http_err}') - except Exception as err: - print(f'Other error: {err}') - return False -``` - -- `stop`: Commands the robot to stop moving via a POST request. -```python - def stop(self, robot_name: str): - ''' Command the robot to stop. - Return True if robot has successfully stopped. Else False''' - url = self.prefix +\ - f'/open-rmf/rmf_demos_fm/stop_robot?robot_name={robot_name}' - try: - response = requests.get(url, self.timeout) - response.raise_for_status() - if self.debug: - print(f'Response: {response.json()}') - return response.json()['success'] - except HTTPError as http_err: - print(f'HTTP error: {http_err}') - except Exception as err: - print(f'Other error: {err}') - return False -``` - -- `navigation_remaining_duration`: Retrieves the remaining duration for the robot to complete its current navigation. - -```python - def navigation_remaining_duration(self, robot_name: str): - ''' Return the number of seconds remaining for the robot to reach its - destination''' - response = self.data(robot_name) - if response is not None: - return response['data']['destination_arrival_duration'] - else: - return 0.0 -``` - -- `process_completed`: Checks if the robot has completed the ongoing process or task. This implementation uses an additional `navigation_completed` method to perform a GET request on the process status. - -```python - def navigation_completed(self, robot_name: str): - ''' Return True if the robot has successfully completed its previous - navigation request. Else False.''' - response = self.data(robot_name) - if response is not None and response.get('data') is not None: - return response['data']['completed_request'] - else: - return False - - def process_completed(self, robot_name: str): - ''' Return True if the robot has successfully completed its previous - process request. Else False.''' - return self.navigation_completed(robot_name) -``` - -Further parameters may be added to `RobotAPI` to be used in these callbacks if required, such as authentication details and task IDs. You may also wish to write additional methods in `RobotAPI` and call them in your `fleet_adapter.py` for specific use cases. - -## 5. Create your fleet adapter! - -Now that we have our components ready, we can start writing our fleet adapter. You may either - -- Use the Full Control Python fleet adapter as a starting point and optionally add customization to its logic by modifying the RobotCommandHandle code, or -- Use our out-of-the-box fleet adapter logic with Easy Full Control with the [`easy_fleet_adapter`](https://github.com/open-rmf/fleet_adapter_template/blob/xiyu/update_EFC/fleet_adapter_template/fleet_adapter_template/easy_fleet_adapter.py) template provided in `fleet_adapter_template`. - -The following steps elaborate on how an **Easy Full Control** adapter can be written. It uses the same C++ API as the Full Control fleet adapter, with an additional layer of the [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class that helps to set up the fleet adapter's internal logic without user intervention. You will only need to parse your `config.yaml` and navigation graphs, as well as have some required callback functions ready, to create and start using your fleet adapter. - -Some code snippets from the [`easy_fleet_adapter.py`](https://github.com/open-rmf/rmf_demos/blob/feature/easy_full_control/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/easy_fleet_adapter.py) implemented in [rmf_demos_fleet_adapter](https://github.com/open-rmf/rmf_demos/tree/main/rmf_demos_fleet_adapter) are provided as examples. - -### 5a. Create a `FleetAdapter` class - -For ease of implementation, we create a `FleetAdapter` class to keep track of some fleet parameters and to spin up our adapter node easily. - -```python -class FleetAdapter: - - def __init__(self, config_path, nav_graph_path, node, use_sim_time): - # Load config yaml - with open(config_path, "r") as f: - config_yaml = yaml.safe_load(f) - # Initialize robot API for this fleet - fleet_config = config_yaml['rmf_fleet'] - prefix = 'http://' + fleet_config['fleet_manager']['ip'] + \ - ':' + str(fleet_config['fleet_manager']['port']) - self.api = RobotAPI( - prefix, - fleet_config['fleet_manager']['user'], - fleet_config['fleet_manager']['password']) - - node.declare_parameter('server_uri', rclpy.Parameter.Type.STRING) - server_uri = node.get_parameter( - 'server_uri').get_parameter_value().string_value - if server_uri == "": - server_uri = None -``` - -Here we load the fleet configuration and navigation graph YAML files, and set up access to the `RobotAPI` class that we updated previously. The `server_uri` parameter is also initialized here to be used with RMF Web. - -```python - # Create EasyFullControl adapter - self.configuration = adpt.easy_full_control.Configuration.make( - config_path, nav_graph_path, server_uri) - self.adapter = self.initialize_fleet( - self.configuration, config_yaml['robots'], node, use_sim_time) -``` - -We then create the `Configuration` object that will be required for the instantiation of the fleet adapter. This object encapsulates all the information about your fleet configuration and navigation graph and passes them on easily to the fleet adapter. - -Now we are ready to make the Easy Full Control adapter, which will be done in the `initialize_fleet` method. - -### 5b. Make the adapter - -```python - def initialize_fleet(self, configuration, robots_yaml, node, use_sim_time): - # Make the easy full control - easy_full_control = adpt.EasyFullControl.make(configuration) - - if use_sim_time: - easy_full_control.node.use_sim_time() -``` - -Inside the `initialize_fleet` method, we create the fleet adapter by passing our `Configuration` object to `EasyFullControl`'s `make(~)` function. - -### 5c. Create individual robot callbacks - -Before we can add our robots to the fleet, we need to define callbacks for each of them to perform various functions. These callbacks are needed for RMF to -- Retrieve the robot's state information, such as charger name, current location, battery state of charge, etc. (`GetStateCallback`) -- Send navigation (`NavigationRequest`), docking (`DockRequest`) or action (`ActionExecutor`) commands to the robot -- Request for the robot to stop moving (`StopRequest`) - -They are defined in the [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class, take a moment to look at the functions and the arguments they should take before implementing your own. - -If you have multiple robots to be added to your fleet that use the same callbacks, you can opt to create a generic function that takes in an identification argument (e.g. robot name) to avoid re-writing similar code. Some examples from `rmf_demos_fleet_adapter` below show how they can be implemented. Here we use parameters like `self.cmd_id` and `self.actions` to keep track of tasks and processes being carried out for the demos integration. - -**RobotState** - -The `GetStateCallback` function will be called at every update interval for RMF to keep tabs on the robot's whereabouts and availability for task allocation. It returns a [`RobotState`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp#L230) object or `None` if the robot state cannot be queried. - -```python - def _robot_state(robot_name): - # Use the RobotAPI to retrieve the robot's state information - data = self.api.data(robot_name) - - # If there is no information available, return None - if data is None or data['success'] is False: - return None - - # Return the robot information in a RobotState object - pos = data['data']['position'] - action = False - if robot_name in self.actions and \ - self.actions[robot_name] is not None: - action = True - state = adpt.easy_full_control.RobotState( - robot, - robot_config['charger']['waypoint'], - data['data']['map_name'], - [pos['x'], pos['y'], pos['yaw']], - data['data']['battery'], - action) - self.last_map[robot_name] = data['data']['map_name'] - return state -``` - -**Navigation** - -The `NavigationRequest` callback requires users to trigger a robot-specific navigation command, and call `execution.finished()` when the robot has reached its target waypoint. - -```python - def _navigate(robot_name, map_name, goal, execution): - # Clear current ongoing navigations if any - if robot_name in self.nav_threads: - if self.nav_threads[robot_name] is not None: - if self.nav_threads[robot_name].is_alive(): - self.nav_threads[robot_name].join() - - cmd_id = self.next_id - self.next_id += 1 - self.cmd_ids[robot_name] = cmd_id - - # Use RobotAPI to send navigation commands - self.api.navigate(robot_name, cmd_id, goal, map_name) - - # Internal tracking that calls execution.finished() - # when the robot reaches the destination - with self._lock: - self.nav_threads[robot_name] = None - self.nav_threads[robot_name] = threading.Thread( - target=self.start_navigation, - args=(robot_name, execution)) - self.nav_threads[robot_name].start() -``` - -In `rmf_demos_fleet_adapter`, this is done by passing the execution object to an internal function `start_navigation()` that continually checks the robot's navigation progress using RobotAPI's `navigate()` method. You can also use `execution.update_request()` to ask RMF to replan its trajectory and update the remaining duration for the navigation task. - -```python - def start_navigation(self, - robot_name, - execution): - while not self.api.process_completed(robot_name, - self.cmd_ids[robot_name]): - remaining_time = self.api.navigation_remaining_duration( - robot_name, self.cmd_ids[robot_name]) - if remaining_time: - remaining_time = datetime.timedelta(seconds=remaining_time) - request_replan = self.api.requires_replan(robot_name) - execution.update_request(request_replan, remaining_time) - # Navigation completed - execution.finished() -``` - -**DockRequest** - -Similarly, the `DockRequest` callback should call RobotAPI's `dock()` method and keep track of the robot's docking progress, and trigger `execution.finished()` when it is done with the task. - -```python - def _dock(robot_name, dock_name, execution): - if dock_name not in self.docks: - node.get_logger().info( - f'Requested dock {dock_name} not found, ' - f'ignoring docking request') - return - - # Clear current ongoing docking if any - if robot_name in self.traj_threads: - if self.traj_threads[robot_name] is not None: - if self.traj_threads[robot_name].is_alive(): - self.traj_threads[robot_name].join() - - cmd_id = self.next_id - self.next_id += 1 - self.cmd_ids[robot_name] = cmd_id - self.api.start_process( - robot_name, cmd_id, dock_name, self.last_map[robot_name]) - - positions = [] - for wp in self.docks[dock_name]: - positions.append([wp.x, wp.y, wp.yaw]) - task_completed_cb = self.api.navigation_completed - node.get_logger().info( - f"Robot {robot_name} is docking at {dock_name}...") - - with self._lock: - self.traj_threads[robot_name] = None - self.traj_threads[robot_name] = threading.Thread( - target=self.start_trajectory, - args=(task_completed_cb, robot_name, positions, - execution.handle(), execution)) - self.traj_threads[robot_name].start() -``` - -In this example, we have an internal function `start_trajectory()` to check on the docking process. We also made use of RMF adapter's scheduling tools to create trajectories for this request, allowing for additional monitoring of the robot. - -```python - # Track trajectory of docking or custom actions - def start_trajectory(self, - task_completed_cb, - robot_name, - positions, - update_handle, - execution): - while not task_completed_cb(robot_name, self.cmd_ids[robot_name]): - now = datetime.datetime.fromtimestamp(0) + \ - self.adapter.node.now() - traj = schedule.make_trajectory( - self.configuration.vehicle_traits(), - now, - positions) - itinerary = schedule.Route(self.last_map[robot_name], traj) - if update_handle is not None: - participant = update_handle.get_unstable_participant() - participant.set_itinerary([itinerary]) - execution.finished() -``` - -**Custom Actions** - -If you added custom actions to your `config.yaml`, you can specify the logic and trigger for the action using the `ActionExecutor` callback. As usual, you will have to call `execution.finished()` to signal the end of the performed action. The action name that you added will be carried here under `category`. This example stores the `ActionExecution` object in `self.actions`, and then uses an additional `toggle_action()` API to relay to the fleet manager that it can start on the given action. - -```python - def _action_executor( - robot_name: str, - category: str, - description: dict, - execution: adpt.robot_update_handle.ActionExecution): - self.actions[robot_name] = execution - self.api.toggle_action(robot_name, True) -``` - -The sections [User-defined Task](./task_userdefined.md) and [Supporting a New Task](./task_new.md) of this Book explain in further detail how an `ActionExecutor` function can be written and how a custom task can be created respectively. - -**Stop** - -`StopRequest` simply calls RobotAPI's corresponding method to request for the robot to stop moving. - -```python - def _stop(robot_name): - cmd_id = self.next_id - self.next_id += 1 - self.cmd_ids[robot_name] = cmd_id - return self.api.stop(robot_name, cmd_id) -``` - -### 5d. Add robot to the fleet adapter - -To add robots to the fleet adapter, you will need to pass the robot's starting state and the callbacks defined above to `EasyFullControl`'s `add_robot(~)` method. Here we keep adding the robots listed in our `config.yaml` until we found all of them. - -```python - # Add the robots - missing_robots = robots_yaml - while len(missing_robots) > 0: - for robot in list(missing_robots.keys()): - node.get_logger().info(f'Connecting to robot [{robot}]') - robot_config = robots_yaml[robot]['rmf_config'] - state = _robot_state(robot) - if state is None: - node.get_logger().info(f'Unable to find robot [{robot}], trying again...') - time.sleep(0.2) - continue - # Found robot, add to fleet - easy_full_control.add_robot( - state, - partial(_robot_state, robot), - partial(_navigate, robot), - partial(_stop, robot), - partial(_dock, robot), - partial(_action_executor, robot)) - node.get_logger().info(f'Successfully added new robot: [{robot}]') - del missing_robots[robot] - - return easy_full_control -``` - -### 5e. Initialize the `FleetAdapter` - -With the `FleetAdapter` set up, we can create our adapter instance. We take in the configuration and navigation graph file paths as command line arguments and pass them to the `FleetAdapter` class along with a ROS 2 node for the command handle. Additionally you might want to use the `use_sim_time` parameter if you would like to run the adapter in simulation. - -```python -def main(argv=sys.argv): - # Init rclpy and adapter - rclpy.init(args=argv) - adpt.init_rclcpp() - args_without_ros = rclpy.utilities.remove_ros_args(argv) - - parser = argparse.ArgumentParser( - prog="fleet_adapter", - description="Configure and spin up the fleet adapter") - parser.add_argument("-c", "--config_file", type=str, required=True, - help="Path to the config.yaml file") - parser.add_argument("-n", "--nav_graph", type=str, required=True, - help="Path to the nav_graph for this fleet adapter") - parser.add_argument("-sim", "--use_sim_time", action="store_true", - help='Use sim time, default: false') - args = parser.parse_args(args_without_ros[1:]) - print(f"Starting fleet adapter...") - - config_path = args.config_file - nav_graph_path = args.nav_graph - - # Load config and nav graph yamls - with open(config_path, "r") as f: - config_yaml = yaml.safe_load(f) - - # ROS 2 node for the command handle - fleet_name = config_yaml['rmf_fleet']['name'] - node = rclpy.node.Node(f'{fleet_name}_command_handle') - - # Enable sim time for testing offline - if args.use_sim_time: - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) - node.set_parameters([param]) - - adapter = FleetAdapter( - config_path, - nav_graph_path, - node, - args.use_sim_time) - - # Create executor for the command handle node - rclpy_executor = rclpy.executors.SingleThreadedExecutor() - rclpy_executor.add_node(node) - - # Start the fleet adapter - rclpy_executor.spin() - - # Shutdown - node.destroy_node() - rclpy_executor.shutdown() - rclpy.shutdown() - - -if __name__ == '__main__': - main(sys.argv) -``` \ No newline at end of file From 36c2eb5782413db5bcaf0b6a6cba41754391775c Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Wed, 4 Oct 2023 00:28:24 +0800 Subject: [PATCH 8/9] Use main branch in links Signed-off-by: Xiyu Oh --- src/integration_fleets.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/integration_fleets.md b/src/integration_fleets.md index 9e0e54b..b00f4eb 100644 --- a/src/integration_fleets.md +++ b/src/integration_fleets.md @@ -17,7 +17,7 @@ The C++ API for **Full Control** automated guided vehicle (AGV) fleets can be fo * [`RobotUpdateHandle`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) - Use this to update the position of a robot and to notify the adapter if the robot's progress gets interrupted. * [`RobotCommandHandle`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotCommandHandle.hpp) - This is a pure abstract interface class. The functions of this class must be implemented to call upon the API of the specific fleet manager that is being adapted. -The C++ API for **Easy Full Control** fleets provides a simple and more accessible way for users to integrate with the Full Control library without having to modify its internal logic. It can be found in the [rmf_fleet_adapter](https://github.com/open-rmf/rmf_ros2/tree/feature/easy_full_control/rmf_fleet_adapter) package of the `rmf_ros2` repo. The [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/feature/easy_full_control/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class contains helpful methods for users to create a `Configuration` object from YAML files encapsulating important fleet configuration parameters and navigation graphs, as well as to make their own fleet adapter with the `Configuration` object. The `add_robot(~)` method is provided for users to add robots to the new fleet adapter. This method takes in various callbacks that should be written by the user, and will be triggered whenever RMF is retrieving robot state information from the fleet or sending out commands to perform a particular process (navigation, docking, action, etc.). An example of the EasyFullControl fleet adapter can be found in [`easy_fleet_adapter.py`](https://github.com/open-rmf/rmf_demos/blob/feature/easy_full_control/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/easy_fleet_adapter.py) under the `rmf_demos` repo. +The C++ API for **Easy Full Control** fleets provides a simple and more accessible way for users to integrate with the Full Control library without having to modify its internal logic. It can be found in the [rmf_fleet_adapter](https://github.com/open-rmf/rmf_ros2/tree/main/rmf_fleet_adapter) package of the `rmf_ros2` repo. The [`EasyFullControl`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp) class contains helpful methods for users to create a `Configuration` object from YAML files encapsulating important fleet configuration parameters and navigation graphs, as well as to make their own fleet adapter with the `Configuration` object. The `add_robot(~)` method is provided for users to add robots to the new fleet adapter. This method takes in various callbacks that should be written by the user, and will be triggered whenever RMF is retrieving robot state information from the fleet or sending out commands to perform a particular process (navigation, docking, action, etc.). An example of the EasyFullControl fleet adapter can be found in [`fleet_adapter.py`](https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py) under the `rmf_demos` repo. The C++ API for **Traffic Light Control** fleets (i.e. fleets that only allow RMF to pause/resume each mobile robot) can also be found in the `rmf_fleet_adapter` package of the `rmf_ros2` repo. The API reuses the `Adapter` class and requires users to initialize their fleet using either of the APIs [here](https://github.com/open-rmf/rmf_ros2/blob/9b4b8a8cc38b323f875a55c70f307446584d1639/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L106-L180). The user has the option to integrate via the [`TrafficLight`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/TrafficLight.hpp) API or for greater convenience, via the [`EasyTrafficLight`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyTrafficLight.hpp) API. From aadb22dc519ef5e7b028c29f0068b8201f075059 Mon Sep 17 00:00:00 2001 From: Xiyu Oh Date: Fri, 27 Oct 2023 16:05:03 -0700 Subject: [PATCH 9/9] Review comments Signed-off-by: Xiyu Oh --- src/integration_fleets_adapter_tutorial.md | 32 ++++++++++++++-------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/src/integration_fleets_adapter_tutorial.md b/src/integration_fleets_adapter_tutorial.md index d286f1f..4422f38 100644 --- a/src/integration_fleets_adapter_tutorial.md +++ b/src/integration_fleets_adapter_tutorial.md @@ -26,10 +26,15 @@ The `fleet_adapter` receives information (position, current ongoing tasks, batte ### Fetch dependencies -Since the `rmf_demos_fleet_adapter` uses REST API as an interface between the fleet adapter and robot fleet manager, we will need to install the required dependencies to use FastAPI. +Before running your fleet adapter, make sure that you have ROS 2 and RMF installed by following the instructions [here](./installation.md). You have the option of installing the binaries or building from source for both. You may also wish to head over to our [RMF Github repo](https://github.com/open-rmf/rmf) for the latest updates and instructions for RMF installation. + +If you built ROS 2 and/or RMF from source, make sure to source the workspace that contains their built code before proceeding to the next step. + +In our example, the `rmf_demos_fleet_adapter` uses REST API as an interface between the fleet adapter and robot fleet manager, hence to get the demos working we will need to install the required dependencies to use FastAPI. ```bash pip3 install fastapi uvicorn ``` +This step is only required for this implementation; depending on what API your own fleet manager uses, you'll have to install any necessary dependencies accordingly. ### Get started with the fleet adapter template @@ -194,13 +199,7 @@ At this point, you should have 4 components ready in order to run your fleet ada - Fleet `config.yaml` file - Navigation graph -### a. Install ROS 2 and sRMF - -Before running your fleet adapter, make sure that you have ROS 2 and RMF installed by following the instructions [here](./installation.md). You have the option of installing the binaries or building from source for both. You may also wish to head over to our [RMF Github repo](https://github.com/open-rmf/rmf) for the latest updates and instructions for RMF installation. - -If you built ROS 2 and/or RMF from source, make sure to source the workspace that contains their built code before proceeding to the next step. - -### b. Build your fleet adapter package +### Build your fleet adapter package If you cloned the `fleet_adapter_template` repository, you would already have your Python scripts in a ROS 2 package. Otherwise, you can follow the instructions [here](https://docs.ros.org/en/rolling/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html) to create a package in your workspace. For the instructions below, we will use the package and module names used in the `fleet_adapter_template` package. @@ -210,7 +209,7 @@ With your scripts in the appropriate folder, go back to the root directory of yo colcon build --packages-select fleet_adapter_template ``` -### c. Run! +### Run! We will now source our workspace and run the fleet adapter: @@ -295,7 +294,16 @@ def compute_transforms(level, coords, node=None): ) ``` -Then, in our `main` function, we at the computed transforms to our fleet configuration. The EasyFullControl fleet adapter will process these transforms and send out navigation commands in the robot's coordinates accordingly. +```python + # Configure the transforms between robot and RMF frames + for level, coords in config_yaml['reference_coordinates'].items(): + tf = compute_transforms(level, coords, node) + fleet_config.add_robot_coordinates_transformation(level, tf) +``` + +Depending on the number of maps (or levels) required for your integration, you will extract the corresponding coordinate transformations for each map and add them to the FleetConfiguration object. The transformation error estimate will be logged by this function if you pass your `rclpy.Node` into it. + +Then, in our `main` function, we add the computed transforms to our FleetConfiguration. The EasyFullControl fleet adapter will process these transforms and send out navigation commands in the robot's coordinates accordingly. ```python # Configure the transforms between robot and RMF frames @@ -306,7 +314,7 @@ Then, in our `main` function, we at the computed transforms to our fleet configu ### c. Initialize the robot API and set up RobotAdapter -The `config.yaml` should include any connection credentials we'd need to connect to our robot or robot fleet manager. We parse this to the `RobotAPI` to easily interact between RMF and the robot's API. +The `config.yaml` may include any connection credentials we'd need to connect to our robot or robot fleet manager. We parse this to the `RobotAPI` to easily interact between RMF and the robot's API. This is entirely optional; for more secure storage of credentials, do import them into RobotAPI accordingly. ```python # Initialize robot API for this fleet @@ -461,3 +469,5 @@ Notice that `execute_action(~)` does not have any implemented code in the fleet ) ) ``` + +Finally, we add all of our callbacks to our fleet adapter using the `RobotCallbacks()` API.