From d1c13168c7e8f946f88a61b4115738efc1b0b841 Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Thu, 1 Sep 2022 14:01:49 +0530 Subject: [PATCH 01/19] Cpp Tutorial for fleet adapter. Signed-off-by: Dev Manek --- src/SUMMARY.md | 2 + src/integration_fleets_tutorial.md | 324 +--------------------- src/integration_fleets_tutorial_cpp.md | 102 +++++++ src/integration_fleets_tutorial_python.md | 322 +++++++++++++++++++++ 4 files changed, 430 insertions(+), 320 deletions(-) create mode 100644 src/integration_fleets_tutorial_cpp.md create mode 100644 src/integration_fleets_tutorial_python.md diff --git a/src/SUMMARY.md b/src/SUMMARY.md index 31dd9ba..8d75e50 100644 --- a/src/SUMMARY.md +++ b/src/SUMMARY.md @@ -22,6 +22,8 @@ - [Navigation Maps](./integration_nav-maps.md) - [Mobile Robot Fleets](./integration_fleets.md) - [Tutorials](./integration_fleets_tutorial.md) + - [Python Tutorial](./integration_fleets_tutorial_python.md) + - [Cpp Tutorial](./integration_fleets_tutorial_cpp.md) - [Free Fleet](./integration_free-fleet.md) - [Read-Only Fleets](./integration_read-only.md) - [Doors](./integration_doors.md) diff --git a/src/integration_fleets_tutorial.md b/src/integration_fleets_tutorial.md index 9055639..896299f 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -1,4 +1,4 @@ -# Fleet Adapter Tutorial (Python) +# Fleet Adapter Tutorial `fleet_adapter` acts as a bridge between the robots and the core RMF system. @@ -20,323 +20,7 @@ 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. +## Tutorials -## Fetch dependencies - -```bash - -pip3 install fastapi uvicorn - -``` - -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. - -> The code given below serves as an example for implementing your own fleet adapter using `RobotClientAPI.py`. - -## 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. - -```yaml -# FLEET CONFIG ================================================================= -# RMF Fleet parameters - -rmf_fleet: - name: "tinyRobot" - fleet_manager: - ip: "127.0.0.1" - port: 22011 - user: "some_user" - password: "some_password" - 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 - clean: False - finishing_request: "park" # [park, charge, nothing] - -# TinyRobot 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: - 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: "tinyRobot2_charger" - orientation: 0.0 # radians - charger: - waypoint: "tinyRobot2_charger" - -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. 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 that can enable/disable reverse traversal in the robot. - -- `battery_system` information about the battery - -- `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` - -- `robots` information about individual fleet robots - -- `tinyRobot1`, `tinyRobot2` name of the robot. - -- `max_delay` seconds before interruption occurs and replanning happens - -- `robot_state_update_frequency` how frequently should the robot update the fleet - -- `start` specify 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` 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. - -## RobotClientAPI.py - -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. - -```python - -import requests -from urllib.error import HTTPError - - -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 - -``` - -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 - - def check_connection(self): - ''' Return True if connection to the robot API server is successful''' - if self.data() is None: - return False - return True - -``` - -[`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 - -```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 - -``` - -[`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 - -```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 - -``` - -[`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. - -```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 - - 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 - -``` - -[`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. - -```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 - - 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) - - 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 +- [Python Tutorial](./integration_fleets_tutorial_python.md) +- [Cpp Tutorial](./integration_fleets_tutorial_cpp.md) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md new file mode 100644 index 0000000..685c556 --- /dev/null +++ b/src/integration_fleets_tutorial_cpp.md @@ -0,0 +1,102 @@ +# Fleet Adapter C++ Tutorial + +> The files mentioned in this tutorial can be found [here](https://github.com/open-rmf/rmf_ros2/tree/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv) + +## [Adapter.hpp](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp) + +```cpp +#ifndef RMF_FLEET_ADAPTER__AGV__ADAPTER_HPP +#define RMF_FLEET_ADAPTER__AGV__ADAPTER_HPP + +#include +#include + +#include +#include + +#include + +namespace rmf_fleet_adapter { +namespace agv { +class Adapter : public std::enable_shared_from_this +{ +public: + + static std::shared_ptr init_and_make( + const std::string& node_name, + std::optional discovery_timeout = std::nullopt); + + static std::shared_ptr make( + const std::string& node_name, + const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions(), + std::optional discovery_timeout = std::nullopt); + + std::shared_ptr add_fleet( + const std::string& fleet_name, + rmf_traffic::agv::VehicleTraits traits, + rmf_traffic::agv::Graph navigation_graph, + std::optional server_uri = std::nullopt); + + using Blockers = std::vector; + + void add_easy_traffic_light( + std::function handle_callback, + const std::string& fleet_name, + const std::string& robot_name, + rmf_traffic::agv::VehicleTraits traits, + std::function pause_callback, + std::function resume_callback, + std::function deadlock_callback = nullptr); + + + /// Get the rclcpp::Node that this adapter will be using for communication. + std::shared_ptr node(); + + /// const-qualified node() + std::shared_ptr node() const; + + /// Begin running the event loop for this adapter. The event loop will operate + /// in another thread, so this function is non-blocking. + Adapter& start(); + + /// Stop the event loop if it is running. + Adapter& stop(); + + /// Wait until the adapter is done spinning. + /// + /// \sa wait_for() + Adapter& wait(); + + /// Wait until the adapter is done spinning, or until the maximum wait time + /// duration is reached. + /// + /// \sa wait() + Adapter& wait_for(std::chrono::nanoseconds max_wait); + + class Implementation; +private: + Adapter(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +using AdapterPtr = std::shared_ptr; +using ConstAdapterPtr = std::shared_ptr; + +} // namespace agv +} // namespace rmf_fleet_adapter + +#endif // RMF_FLEET_ADAPTER__AGV__ADAPTER_HPP + +``` + +[`init_and_make`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L51) will initialize rclcpp context and makes an adapter instance.You can add the fleets to be adapted. +takes two arguments + +- node_name - The name of rclcpp node that will be produced for this adapter. +- discovery_timeout - The minimum time it will wait to discover the schedule node before giving up. If the default `rmf_utils:nullopt` is used it will use `discovery_timeout`node parameter or wait for 1 minute if`discovery_timeout` is not defined. + +[`make`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L75) instantiates an rclcpp::Noe like `init_and_make` however it is more customisable. + +- node_name - The name of rclcpp node that will be produced for this adapter. +- +- discovery_timeout - The minimum time it will wait to discover the schedule node before giving up. If the default `rmf_utils:nullopt` is used it will use `discovery_timeout`node parameter or wait for 1 minute if`discovery_timeout` is not defined. diff --git a/src/integration_fleets_tutorial_python.md b/src/integration_fleets_tutorial_python.md new file mode 100644 index 0000000..a3d5d7b --- /dev/null +++ b/src/integration_fleets_tutorial_python.md @@ -0,0 +1,322 @@ +# Fleet Adapter Python Tutorial + +> 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 + +```bash + +pip3 install fastapi uvicorn + +``` + +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. + +> The code given below serves as an example for implementing your own fleet adapter using `RobotClientAPI.py`. + +## 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. + +```yaml +# FLEET CONFIG ================================================================= +# RMF Fleet parameters + +rmf_fleet: + name: "tinyRobot" + fleet_manager: + ip: "127.0.0.1" + port: 22011 + user: "some_user" + password: "some_password" + 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 + clean: False + finishing_request: "park" # [park, charge, nothing] + +# TinyRobot 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: + 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: "tinyRobot2_charger" + orientation: 0.0 # radians + charger: + waypoint: "tinyRobot2_charger" + +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. 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 that can enable/disable reverse traversal in the robot. + +- `battery_system` information about the battery + +- `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` + +- `robots` information about individual fleet robots + +- `tinyRobot1`, `tinyRobot2` name of the robot. + +- `max_delay` seconds before interruption occurs and replanning happens + +- `robot_state_update_frequency` how frequently should the robot update the fleet + +- `start` specify 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` 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. + +## RobotClientAPI.py + +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. + +```python + +import requests +from urllib.error import HTTPError + + +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 + +``` + +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 + + def check_connection(self): + ''' Return True if connection to the robot API server is successful''' + if self.data() is None: + return False + return True + +``` + +[`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 + +```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 + +``` + +[`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 + +```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 + +``` + +[`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. + +```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 + + 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 + +``` + +[`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. + +```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 + + 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) + + 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 From ade7f9726cc39af84bbdf2bc273782e863396def Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Sun, 11 Sep 2022 21:21:38 +0530 Subject: [PATCH 02/19] Adds cpp integration_fleets_tutorial_cpp Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 29 +++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 685c556..04afe87 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -95,8 +95,31 @@ takes two arguments - node_name - The name of rclcpp node that will be produced for this adapter. - discovery_timeout - The minimum time it will wait to discover the schedule node before giving up. If the default `rmf_utils:nullopt` is used it will use `discovery_timeout`node parameter or wait for 1 minute if`discovery_timeout` is not defined. -[`make`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L75) instantiates an rclcpp::Noe like `init_and_make` however it is more customisable. +[`make`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L75) instantiates an rclcpp::Node like `init_and_make` however it is more customisable. - node_name - The name of rclcpp node that will be produced for this adapter. -- -- discovery_timeout - The minimum time it will wait to discover the schedule node before giving up. If the default `rmf_utils:nullopt` is used it will use `discovery_timeout`node parameter or wait for 1 minute if`discovery_timeout` is not defined. +- node_options - It can be used to customize the node. +- discovery_timeout - The minimum time it will wait to discover the schedule node before giving up. If the default `rmf_utils:nullopt` is used it will use `discovery_timeout` node parameter or wait for 1 minute if `discovery_timeout` is not defined. + +`add_fleet` allows to add a fleet that needs to be adapted. + +- fleet_name - name of the fleet +- traits - specify approximate traits of the vehicle. +- navigation_graph - specify navigation graph used by the vehicles in the fleet +- server_uri - uri of the websocket server that receives updates on the tasks and states. If the default `rmf_utils:nullopt` is used it will not publish any data. + +`add_easy_traffic_light` helps you create simple version of traffic light which allows to manage robot that can only support pause or resume commands.This API is simpler to use than the standard traffic light API but it provides less information about the exact timing needed for the starts and stops.This API must only be used if system integrators can ensure very low-latency and reliable connections to the robots to ensure that commands arrive on time. + +- handle_callback -The callback that will be triggered when the EasyTrafficLight handle is ready to be used. This callback will only be triggered once. + +- fleet_name - The name of the fleet + +- robot_name -The name of the robot + +- traits -The traits of the robot + +- pause_callback -The callback that should be triggered by the traffic light when an immediate pause is needed. + +- resume_callback -The callback that will be triggered by the traffic light when the robot may resume moving forward. + +- deadlock_callbackThe callback that will be triggered by the traffic light if there is a permanent blocker disrupting the ability of this vehicle to proceed.Manual intervention may be required in this circumstance. A callback does not need to be provided for this. Either way, an error message will be printed to the log. From d92e7a0640e5f97ecb8bb1f4e2821eb0a302482a Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Mon, 12 Sep 2022 11:09:51 +0530 Subject: [PATCH 03/19] Adds information about `FleetUpdateHandle.hpp` Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 238 +++++++++++++++++++++++++ 1 file changed, 238 insertions(+) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 04afe87..e66fa45 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -123,3 +123,241 @@ takes two arguments - resume_callback -The callback that will be triggered by the traffic light when the robot may resume moving forward. - deadlock_callbackThe callback that will be triggered by the traffic light if there is a permanent blocker disrupting the ability of this vehicle to proceed.Manual intervention may be required in this circumstance. A callback does not need to be provided for this. Either way, an error message will be printed to the log. + +## [FleetUpdateHandle.hpp](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp) + +The Update Handle works as handler between the fleet robots and the fleetadapter. + +```c++ +#ifndef RMF_FLEET_ADAPTER__AGV__FLEETUPDATEHANDLE_HPP +#define RMF_FLEET_ADAPTER__AGV__FLEETUPDATEHANDLE_HPP + +#include +#include + +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace rmf_fleet_adapter { +namespace agv { + +//============================================================================== +class FleetUpdateHandle : public std::enable_shared_from_this +{ +public: + + void add_robot( + std::shared_ptr command, + const std::string& name, + const rmf_traffic::Profile& profile, + rmf_traffic::agv::Plan::StartSet start, + std::function handle)> handle_cb); + + /// Confirmation is a class used by the task acceptance callbacks to decide if + /// a task description should be accepted. + class Confirmation + { + public: + + /// Constructor + Confirmation(); + + /// Call this function to decide that you want to accept the task request. + /// If this function is never called, it will be assumed that the task is + /// rejected. + Confirmation& accept(); + + /// Check whether + bool is_accepted() const; + + /// Call this function to bring attention to errors related to the task + /// request. Each call to this function will overwrite any past calls, so + /// it is recommended to only call it once. + Confirmation& errors(std::vector error_messages); + + /// Call this function to add errors instead of overwriting the ones that + /// were already there. + Confirmation& add_errors(std::vector error_messages); + + /// Check the errors that have been given to this confirmation. + const std::vector& errors() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + + using ConsiderRequest = + std::function; + + FleetUpdateHandle& consider_delivery_requests( + ConsiderRequest consider_pickup, + ConsiderRequest consider_dropoff); + + + FleetUpdateHandle& consider_cleaning_requests(ConsiderRequest consider); + + FleetUpdateHandle& consider_patrol_requests(ConsiderRequest consider); + + FleetUpdateHandle& consider_composed_requests(ConsiderRequest consider); + + + FleetUpdateHandle& add_performable_action( + const std::string& category, + ConsiderRequest consider); + + /// Specify a set of lanes that should be closed. + void close_lanes(std::vector lane_indices); + + /// Specify a set of lanes that should be open. + void open_lanes(std::vector lane_indices); + + + bool set_task_planner_params( + std::shared_ptr battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr tool_sink, + double recharge_threshold, + double recharge_soc, + bool account_for_battery_drain, + rmf_task::ConstRequestFactoryPtr finishing_requst = nullptr); + + using AcceptTaskRequest = + std::function; + + using AcceptDeliveryRequest = + std::function; + + /// Specify the default value for how high the delay of the current itinerary + /// can become before it gets interrupted and replanned. A nullopt value will + /// allow for an arbitrarily long delay to build up without being interrupted. + FleetUpdateHandle& default_maximum_delay( + std::optional value); + + /// Get the default value for the maximum acceptable delay. + std::optional default_maximum_delay() const; + + /// The behavior is identical to fleet_state_topic_publish_period + [[deprecated("Use fleet_state_topic_publish_period instead")]] + FleetUpdateHandle& fleet_state_publish_period( + std::optional value); + + /// Specify a period for how often the fleet state message is published for + /// this fleet. Passing in std::nullopt will disable the fleet state message + /// publishing. The default value is 1s. + FleetUpdateHandle& fleet_state_topic_publish_period( + std::optional value); + + /// Specify a period for how often the fleet state is updated in the database + /// and to the API server. This is separate from publishing the fleet state + /// over the ROS2 fleet state topic. Passing in std::nullopt will disable + /// the updating, but this is not recommended unless you intend to provide the + /// API server with the fleet states through some other means. + /// + /// The default value is 1s. + FleetUpdateHandle& fleet_state_update_period( + std::optional value); + + /// Set a callback for listening to update messages (e.g. fleet states and + /// task updates). This will not receive any update messages that happened + /// before the listener was set. + FleetUpdateHandle& set_update_listener( + std::function listener); + + // Do not allow moving + FleetUpdateHandle(FleetUpdateHandle&&) = delete; + FleetUpdateHandle& operator=(FleetUpdateHandle&&) = delete; + + class Implementation; +private: + FleetUpdateHandle(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +using FleetUpdateHandlePtr = std::shared_ptr; +using ConstFleetUpdateHandlePtr = std::shared_ptr; + +} // namespace agv +} // namespace rmf_fleet_adapter + +#endif // RMF_FLEET_ADAPTER__AGV__FLEETUPDATEHANDLE_HPP + +``` + +`add_robot` + +- command + A reference to a command handle for this robot. +- name + The name of this robot. +- profile + The profile of this robot. This profile should account for the largest possible payload that the robot might carry. +- start + The initial location of the robot, expressed as `Plan::StartSet`. Multiple Start objects might be needed if the robot is not starting precisely on a waypoint.The function `rmf_traffic::agv::compute_plan_starts()` may help with this. +- handle_cb + This callback function will get triggered when the RobotUpdateHandle is ready to be used by the Fleet API side of the Adapter. Setting up a new robot requires communication with the Schedule Node, so there may be a delay before the robot is ready to be used. + +- `add_performable_action` + Allow this fleet adapter to execute a PerformAction activity of specified + category which may be present in sequence event. + + - `category` + A string that categorizes the action. This value should be used when + filling out the category field in event_description_PerformAction.json + schema. + + - `consider` + Decide whether to accept the action based on the description field in + event_description_PerformAction.json schema. + +- `set_task_planner_params` + Set the parameters required for task planning. Without calling this + function, this fleet will not bid for and accept tasks. + + - `battery_system` + Specify the battery system used by the vehicles in this fleet. + + - `motion_sink` + Specify the motion sink that describes the vehicles in this fleet. + + - `ambient_sink` + Specify the device sink for ambient sensors used by the vehicles in this fleet. + + - `tool_sink` + Specify the device sink for special tools used by the vehicles in this fleet. + + - `recharge_threshold` + The threshold for state of charge below which robots in this fleet + will cease to operate and require recharging. A value between 0.0 and + 1.0 should be specified. + + - `recharge_soc` + The state of charge to which robots in this fleet should be charged up + to by automatic recharging tasks. A value between 0.0 and 1.0 should be + specified. + + - `account_for_battery_drain` + Specify whether battery drain is to be considered while allocating tasks. + If false, battery drain will not be considered when planning for tasks. + As a consequence, charging tasks will not be automatically assigned to + vehicles in this fleet when battery levels fall below the + recharge_threshold. + + - `finishing_request` + A factory for a request that should be performed by each robot in this + fleet at the end of its assignments. From e6d109cd4d7a388adcb3d735bb9b4e258d10729b Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Thu, 29 Sep 2022 18:50:05 +0530 Subject: [PATCH 04/19] Documentation of two robot classes --- src/integration_fleets_tutorial_cpp.md | 616 ++++++++++++++++++++++--- 1 file changed, 563 insertions(+), 53 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index e66fa45..f46583e 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -89,13 +89,13 @@ using ConstAdapterPtr = std::shared_ptr; ``` -[`init_and_make`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L51) will initialize rclcpp context and makes an adapter instance.You can add the fleets to be adapted. +`init_and_make` will initialize rclcpp context and makes an adapter instance.You can add the fleets to be adapted. takes two arguments - node_name - The name of rclcpp node that will be produced for this adapter. - discovery_timeout - The minimum time it will wait to discover the schedule node before giving up. If the default `rmf_utils:nullopt` is used it will use `discovery_timeout`node parameter or wait for 1 minute if`discovery_timeout` is not defined. -[`make`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp#L75) instantiates an rclcpp::Node like `init_and_make` however it is more customisable. +`make` instantiates an rclcpp::Node like `init_and_make` however it is more customisable. - node_name - The name of rclcpp node that will be produced for this adapter. - node_options - It can be used to customize the node. @@ -111,22 +111,16 @@ takes two arguments `add_easy_traffic_light` helps you create simple version of traffic light which allows to manage robot that can only support pause or resume commands.This API is simpler to use than the standard traffic light API but it provides less information about the exact timing needed for the starts and stops.This API must only be used if system integrators can ensure very low-latency and reliable connections to the robots to ensure that commands arrive on time. - handle_callback -The callback that will be triggered when the EasyTrafficLight handle is ready to be used. This callback will only be triggered once. - - fleet_name - The name of the fleet - - robot_name -The name of the robot - - traits -The traits of the robot - - pause_callback -The callback that should be triggered by the traffic light when an immediate pause is needed. - - resume_callback -The callback that will be triggered by the traffic light when the robot may resume moving forward. - - deadlock_callbackThe callback that will be triggered by the traffic light if there is a permanent blocker disrupting the ability of this vehicle to proceed.Manual intervention may be required in this circumstance. A callback does not need to be provided for this. Either way, an error message will be printed to the log. ## [FleetUpdateHandle.hpp](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp) -The Update Handle works as handler between the fleet robots and the fleetadapter. +The Update Handle works as handler between the fleet robots and the fleetadapter. ```c++ #ifndef RMF_FLEET_ADAPTER__AGV__FLEETUPDATEHANDLE_HPP @@ -300,64 +294,580 @@ using ConstFleetUpdateHandlePtr = std::shared_ptr; ``` `add_robot` +This function will allow you to add a robot to the fleet. -- command +- `command` A reference to a command handle for this robot. -- name +- `name` The name of this robot. -- profile +- `profile` The profile of this robot. This profile should account for the largest possible payload that the robot might carry. -- start - The initial location of the robot, expressed as `Plan::StartSet`. Multiple Start objects might be needed if the robot is not starting precisely on a waypoint.The function `rmf_traffic::agv::compute_plan_starts()` may help with this. -- handle_cb +- `start` The initial location of the robot, expressed as `Plan::StartSet`. Multiple Start objects might be needed if the robot is not starting precisely on a waypoint.The function `rmf_traffic::agv::compute_plan_starts()` may help with this. +- `handle_cb` This callback function will get triggered when the RobotUpdateHandle is ready to be used by the Fleet API side of the Adapter. Setting up a new robot requires communication with the Schedule Node, so there may be a delay before the robot is ready to be used. -- `add_performable_action` - Allow this fleet adapter to execute a PerformAction activity of specified - category which may be present in sequence event. +`add_performable_action` +Allow this fleet adapter to execute a PerformAction activity of specified +category which may be present in sequence event. + +- `category` + A string that categorizes the action. This value should be used when + filling out the category field in event_description_PerformAction.json + schema. +- `consider` + Decide whether to accept the action based on the description field in + event_description_PerformAction.json schema. + +`set_task_planner_params` +Set the parameters required for task planning. Without calling this +function, this fleet will not bid for and accept tasks. + +- `battery_system` + Specify the battery system used by the vehicles in this fleet. +- `motion_sink` + Specify the motion sink that describes the vehicles in this fleet. +- `ambient_sink` + Specify the device sink for ambient sensors used by the vehicles in this fleet. +- `tool_sink` + Specify the device sink for special tools used by the vehicles in this fleet. +- `recharge_threshold` + The threshold for state of charge below which robots in this fleet + will cease to operate and require recharging. A value between 0.0 and + 1.0 should be specified. +- `recharge_soc` + The state of charge to which robots in this fleet should be charged up + to by automatic recharging tasks. A value between 0.0 and 1.0 should be + specified. +- `account_for_battery_drain` + Specify whether battery drain is to be considered while allocating tasks. + If false, battery drain will not be considered when planning for tasks. + As a consequence, charging tasks will not be automatically assigned to + vehicles in this fleet when battery levels fall below the + recharge_threshold. +- `finishing_request` + A factory for a request that should be performed by each robot in this + fleet at the end of its assignments. + +[`RobotUpdateHandle.hpp`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) +The `RobotUpdateHandle` helps in sending updates to the robot. + +```cpp +namespace rmf_fleet_adapter { +namespace agv { + +//============================================================================== +/// You will be given an instance of this class every time you add a new robot +/// to your fleet. Use that instance to send updates to RoMi-H about your +/// robot's state. +class RobotUpdateHandle +{ +public: + + [[deprecated("Use replan() instead")]] + void interrupted(); + + + void replan(); + + void update_position( + std::size_t waypoint, + double orientation); + + void update_position( + const Eigen::Vector3d& position, + const std::vector& lanes); + + void update_position( + const Eigen::Vector3d& position, + std::size_t target_waypoint); + + void update_position( + const std::string& map_name, + const Eigen::Vector3d& position, + const double max_merge_waypoint_distance = 0.1, + const double max_merge_lane_distance = 1.0, + const double min_lane_length = 1e-8); + + void update_position(rmf_traffic::agv::Plan::StartSet position); + + RobotUpdateHandle& set_charger_waypoint(const std::size_t charger_wp); + + void update_battery_soc(const double battery_soc); + + void override_status(std::optional status); + + RobotUpdateHandle& maximum_delay( + rmf_utils::optional value); + + + rmf_utils::optional maximum_delay() const; + + /// The ActionExecution class should be used to manage the execution of and + /// provide updates on ongoing actions. + class ActionExecution + { + public: + /// Update the amount of time remaining for this action + void update_remaining_time(rmf_traffic::Duration remaining_time_estimate); + + void underway(std::optional text); + + void error(std::optional text); + + + void delayed(std::optional text); + + + void blocked(std::optional text); + + void finished(); + + bool okay() const; + + class Implementation; + private: + ActionExecution(); + rmf_utils::impl_ptr _pimpl; + }; + + using ActionExecutor = std::function; + + /// Set the ActionExecutor for this robot + void set_action_executor(ActionExecutor action_executor); + + + void submit_direct_request( + nlohmann::json task_request, + std::string request_id, + std::function receive_response); + + /// An object to maintain an interruption of the current task. When this + /// object is destroyed, the task will resume. + class Interruption + { + public: + /// Call this function to resume the task while providing labels for + /// resuming. + void resume(std::vector labels); + + class Implementation; + private: + Interruption(); + rmf_utils::unique_impl_ptr _pimpl; + }; + + Interruption interrupt( + std::vector labels, + std::function robot_is_interrupted); + + + void cancel_task( + std::string task_id, + std::vector labels, + std::function on_cancellation); + + void kill_task( + std::string task_id, + std::vector labels, + std::function on_kill); + + enum class Tier + { + /// General status information, does not require special attention + Info, + + /// Something unusual that might require attention + Warning, + + /// A critical failure that requires immediate operator attention + Error + }; + + /// An object to maintain an issue that is happening with the robot. When this + /// object is destroyed without calling resolve(), the issue will be + /// "dropped", which issues a warning to the log. + class IssueTicket + { + public: + + /// Indicate that the issue has been resolved. The provided message will be + /// logged for this robot and the issue will be removed from the robot + /// state. + void resolve(nlohmann::json msg); + + class Implementation; + private: + IssueTicket(); + rmf_utils::unique_impl_ptr _pimpl; + }; + + + IssueTicket create_issue( + Tier tier, std::string category, nlohmann::json detail); + + // TODO(MXG): Should we offer a "clear_all_issues" function? + + /// Add a log entry with Info severity + void log_info(std::string text); + + /// Add a log entry with Warning severity + void log_warning(std::string text); + + /// Add a log entry with Error severity + void log_error(std::string text); + + /// Toggle the responsive wait behavior for this robot. When responsive wait + /// is active, the robot will remain in the traffic schedule when it is idle + /// and will negotiate its position with other traffic participants to + /// potentially move out of their way. + /// + /// Disabling this behavior may be helpful to reduce CPU load or prevent + /// parked robots from moving or being seen as conflicts when they are not + /// actually at risk of creating traffic conflicts. + /// + /// By default this behavior is enabled. + void enable_responsive_wait(bool value); + + class Implementation; + + /// This API is experimental and will not be supported in the future. Users + /// are to avoid relying on these feature for any integration. + class Unstable + { + public: + /// True if this robot is allowed to accept new tasks. False if the robot + /// will not accept any new tasks. + bool is_commissioned() const; + + /// Stop this robot from accepting any new tasks. It will continue to + /// perform tasks that are already in its queue. To reassign those tasks, + /// you will need to use the task request API to cancel the tasks and + /// re-request them. + void decommission(); + + /// Allow this robot to resume accepting new tasks. + void recommission(); + + /// Get the schedule participant of this robot + rmf_traffic::schedule::Participant* get_participant(); + + /// Change the radius of the footprint and vicinity of this participant. + void change_participant_profile( + double footprint_radius, + double vicinity_radius); + + /// Override the schedule to say that the robot will be holding at a certain + /// position. This should not be used while tasks with automatic schedule + /// updating are running, or else the traffic schedule will have jumbled up + /// information, which can be disruptive to the overall traffic management. + void declare_holding( + std::string on_map, + Eigen::Vector3d at_position, + rmf_traffic::Duration for_duration = std::chrono::seconds(30)); + + /// Get the current Plan ID that this robot has sent to the traffic schedule + rmf_traffic::PlanId current_plan_id() const; + + /// Hold onto this class to tell the robot to behave as a "stubborn + /// negotiator", meaning it will always refuse to accommodate the schedule + /// of any other agent. This could be used when teleoperating a robot, to + /// tell other robots that the agent is unable to negotiate. + /// + /// When the object is destroyed, the stubbornness will automatically be + /// released. + class Stubbornness + { + public: + /// Stop being stubborn + void release(); + + class Implementation; + private: + Stubbornness(); + rmf_utils::impl_ptr _pimpl; + }; + + /// Tell this robot to be a stubborn negotiator. + Stubbornness be_stubborn(); + + enum class Decision + { + Undefined = 0, + Clear = 1, + Crowded = 2 + }; + + /// A callback with this signature will be given to the watchdog when the + /// robot is ready to enter a lift. If the watchdog passes in a true, then + /// the robot will proceed to enter the lift. If the watchdog passes in a + /// false, then the fleet adapter will release its session with the lift and + /// resume later. + using Decide = std::function; + + using Watchdog = std::function; + + /// Set a callback that can be used to check whether the robot is clear to + /// enter the lift. + void set_lift_entry_watchdog( + Watchdog watchdog, + rmf_traffic::Duration wait_duration = std::chrono::seconds(10)); + + private: + friend Implementation; + Implementation* _pimpl; + }; + + /// Get a mutable reference to the unstable API extension + Unstable& unstable(); + /// Get a const reference to the unstable API extension + const Unstable& unstable() const; + +private: + RobotUpdateHandle(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +using RobotUpdateHandlePtr = std::shared_ptr; +using ConstRobotUpdateHandlePtr = std::shared_ptr; + +} // namespace agv +} // namespace rmf_fleet_adapter + + + +``` + +`update_position` +This function updates the position of the robot by specifying -`waypoint`-The waypoint the robot is on. + +- `orientation` - Current orientation of the robot.' + + or + +- `position` -position of the robot and one or more lanes that the robot is occupying. +- `lanes` - number of lanes(minimum 1 required) + + or + +- `position` -position of the robot and one or more lanes that the robot is occupying. +- `target_waypoint` - number of lanes(minimum 1 required) + This should be used if the robot has diverged significantly from its + course but it is merging back onto a waypoint. + + or + +- `map_name` - The map robot is on +- `position` -position of the robot and one or more lanes that the robot is occupying. + This option should be used if the robot has diverged significantly from its course. + The parameters for this function are passed along to `rmf_traffic::agv::compute_plan_starts()`. + + or + +- `position` The plan set for the robot + +`replan` +The function will tell the RMF schedule that the robot requires a new plan. The new plan will have the starting position of the robot as the last position the robot had updated using `update_position()`. + +`set_charger_waypoint` +Set the waypoint where the charger for this robot is located.If not specified, the nearest waypoint in the graph with the is_charger() +property will be assumed as the charger for this robot. + +- `charger_wp` The waypoint where the charger for this robot is located. + +`update_battery_soc` +Update the current battery level of the robot by specifying its state of charge as a fraction of its total charge capacity. + +`override_status` This function overrides the robot status. + +- `status`The string provided must + be a valid enum as specified in the robot_state.json schema. + Pass std::nullopt to cancel the override and allow RMF to automatically + update the status. The default value is std::nullopt. + +`maximum_delay` +Specify how high the delay of the current itinerary can become before it gets interrupted and replanned. A nullopt value will allow for an +arbitrarily long delay to build up without being interrupted. + +`ActionExecution` + +- `underways` + Set task status to error and optionally log a message (error tier) + +- `errors` + Set the task status to delayed and optionally log a message + (warning tier) +- `delayeds` + Set the task status to blocked and optionally log a message + (warning tier) +- `blockeds` + Trigger this when the action is successfully finished +- `finished` + Returns false if the Action has been killed or cancelled + +`submit_direct_request` + +- `task_request` A JSON description of the task request. It should match the task_request.json schema of rmf_api_msgs, in particular it must contain +- `request_id` The unique ID for this task request. +- `receive_response` Provide a callback to receive the response. The response will be a robot_task_response.json message from rmf_api_msgs (note: this message + +`ActionExecutor` +Signature for a callback to request the robot to perform an action + +- `category` + A category of the action to be performed + +- `description` + A description of the action to be performed + +- `execution` + An ActionExecution object that will be provided to the user for + updating the state of the action. + +`set_action_executor` Set the ActionExecutor for this robot + +`submit_direct_request` Send a request to the robot. + +- `task_request` The json task request +- `request_id` The request ID for the task request +- `receive_response` The response callback + +`interrupt` Interrupt (pause) the current task, yielding control of the robot away +from the fleet adapter's task manager. + +- `labels` + Labels that will be assigned to this interruption. It is recommended to + include information about why the interruption is happening. + +`cancel_task` +Cancel a task, if it has been assigned to this robot + +- `task_id` + The ID of the task to be canceled + +- `labels` + Labels that will be assigned to this cancellation. It is recommended to + include information about why the cancellation is happening. + +- `on_cancellation` + Callback that will be triggered after the cancellation is issued. + task_was_found will be true if the task was successfully found and + issued the cancellation, false otherwise. + +`kill_task` Kill a task, if it has been assigned to this robot + +- `task_id` + The ID of the task to be canceled + +- `labels` + Labels that will be assigned to this cancellation. It is recommended to + include information about why the cancellation is happening. + +- `on_kill` + Callback that will be triggered after the cancellation is issued. + task_was_found will be true if the task was successfully found and + issued the kill, false otherwise. + +`create_issue` Create a new issue for the robot. + +- `tier` + The severity of the issue + +- `category` + A brief category to describe the issue + +- `detail` + Full details of the issue that might be relevant to an operator or + logging system. + +[`RobotCommandHandle`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotCommandHandle.hpp) +This class works as a bridge and acts as a handler for all the commands that need to be sent to the robot. + +```cpp +namespace rmf_fleet_adapter { +namespace agv { + +//============================================================================== +/// Implement this class to receive robot commands from RMF +class RobotCommandHandle +{ +public: + + using Duration = rmf_traffic::Duration; + + using ArrivalEstimator = + std::function; + + /// Trigger this callback function when the follow_new_path request has been + /// completed. It should only be triggered that one time and then discarded. + using RequestCompleted = std::function; + + + virtual void follow_new_path( + const std::vector& waypoints, + ArrivalEstimator next_arrival_estimator, + RequestCompleted path_finished_callback) = 0; + + /// Have the robot come to an immediate stop. + virtual void stop() = 0; + + + virtual void dock( + const std::string& dock_name, + RequestCompleted docking_finished_callback) = 0; + + // Virtual destructor + virtual ~RobotCommandHandle() = default; +}; + +} // namespace agv +} // namespace rmf_fleet_adapter + +``` + +`ArrivalEstimator` +Use this callback function to update the fleet adapter on how long the +robot will take to reach its next destination. + +- `path_index` + The index of the path element that the robot is currently heading + towards. - - `category` - A string that categorizes the action. This value should be used when - filling out the category field in event_description_PerformAction.json - schema. +- `remaining_time` + An estimate of how much longer the robot will take to arrive at + `path_index`. - - `consider` - Decide whether to accept the action based on the description field in - event_description_PerformAction.json schema. +`follow_new_path` Have the robot follow a new path. If it was already following a path, then +it should immediately switch over to this one. -- `set_task_planner_params` - Set the parameters required for task planning. Without calling this - function, this fleet will not bid for and accept tasks. +- `waypoints` + The sequence of waypoints to follow. When the robot arrives at a + waypoint in this sequence, it should wait at that waypoint until the + clock reaches the time() field of the waypoint. This is important + because the waypoint timing is used to avoid traffic conflicts with + other vehicles. - - `battery_system` - Specify the battery system used by the vehicles in this fleet. +- `next_arrival_estimator` + Use this callback to give estimates for how long the robot will take to + reach the path element of the specified index. You should still be + calling RobotUpdateHandle::update_position() even as you call this + function. - - `motion_sink` - Specify the motion sink that describes the vehicles in this fleet. +- `path_finished_callback` + Trigger this callback when the robot is done following the new path. - - `ambient_sink` - Specify the device sink for ambient sensors used by the vehicles in this fleet. +You do not need to trigger waypoint_arrival_callback when triggering +this one. - - `tool_sink` - Specify the device sink for special tools used by the vehicles in this fleet. +`dock` Have the robot begin a pre-defined docking procedure. Implement this +function as a no-op if your robots do not perform docking procedures. - - `recharge_threshold` - The threshold for state of charge below which robots in this fleet - will cease to operate and require recharging. A value between 0.0 and - 1.0 should be specified. +- `dock_name` + The predefined name of the docking procedure to use. - - `recharge_soc` - The state of charge to which robots in this fleet should be charged up - to by automatic recharging tasks. A value between 0.0 and 1.0 should be - specified. +- `docking_finished_callback` + Trigger this callback when the docking is finished. - - `account_for_battery_drain` - Specify whether battery drain is to be considered while allocating tasks. - If false, battery drain will not be considered when planning for tasks. - As a consequence, charging tasks will not be automatically assigned to - vehicles in this fleet when battery levels fall below the - recharge_threshold. - - `finishing_request` - A factory for a request that should be performed by each robot in this - fleet at the end of its assignments. From f7762adec01190d16d5d95b7f34bcfaba201153c Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Thu, 29 Sep 2022 19:14:54 +0530 Subject: [PATCH 05/19] solved discussions/184 --- src/integration_fleets_tutorial_cpp.md | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index f46583e..3ef9653 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -123,25 +123,6 @@ takes two arguments The Update Handle works as handler between the fleet robots and the fleetadapter. ```c++ -#ifndef RMF_FLEET_ADAPTER__AGV__FLEETUPDATEHANDLE_HPP -#define RMF_FLEET_ADAPTER__AGV__FLEETUPDATEHANDLE_HPP - -#include -#include - -#include -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include namespace rmf_fleet_adapter { namespace agv { @@ -334,6 +315,8 @@ function, this fleet will not bid for and accept tasks. The threshold for state of charge below which robots in this fleet will cease to operate and require recharging. A value between 0.0 and 1.0 should be specified. + When the robot is idle, a timer periodically computes the estimated battery drain if the robot were to head back to the charger now. If the battery level after the estimated drain is within a safety factor applied to the recharge_threshold, the TaskManager will automatically begin a task to take the robot back to its charging station. + The recharge_threshold is used in a similar manner during task allocation. When deciding the order of tasks performed, the TaskPlanner will estimate the battery level of the robot after performing a task A. If the estimate is below the recharge_threshold, it will then check if the robot can perform task A if it performed a recharge task before this. If this is the case then it will automatically include a recharge task before task A. If it still cannot perform task A even after recharging, it will discard the possibility of doing task A in that sequence. - `recharge_soc` The state of charge to which robots in this fleet should be charged up to by automatic recharging tasks. A value between 0.0 and 1.0 should be @@ -869,5 +852,3 @@ function as a no-op if your robots do not perform docking procedures. - `docking_finished_callback` Trigger this callback when the docking is finished. - - From 4d664a8684f90b0c7ec8bbcb347aa1e6d65c7db7 Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Thu, 29 Sep 2022 19:17:08 +0530 Subject: [PATCH 06/19] solved discussions/185 --- src/integration_fleets_tutorial_cpp.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 3ef9653..0a5909f 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -276,7 +276,7 @@ using ConstFleetUpdateHandlePtr = std::shared_ptr; `add_robot` This function will allow you to add a robot to the fleet. - +> Note currently there is no support for deleting the robot - `command` A reference to a command handle for this robot. - `name` From 8257ce3499aa26672d667e7936ff71296e5aa60f Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Thu, 29 Sep 2022 19:20:52 +0530 Subject: [PATCH 07/19] solved discussions/180 --- src/integration_fleets_tutorial_cpp.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 0a5909f..500ac98 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -276,7 +276,9 @@ using ConstFleetUpdateHandlePtr = std::shared_ptr; `add_robot` This function will allow you to add a robot to the fleet. + > Note currently there is no support for deleting the robot + - `command` A reference to a command handle for this robot. - `name` @@ -825,6 +827,10 @@ robot will take to reach its next destination. `follow_new_path` Have the robot follow a new path. If it was already following a path, then it should immediately switch over to this one. +> Note:- The waypoints argument that will be given to the follow_new_path function of the RobotCommandHandle contains a list of `rmf_traffic::agv::Plan::Waypoint` objects that have a graph_index function.When that graph_index is non-null it can be passed to `Graph::get_waypoint` and then `Graph::Waypoint::get_map_name` can be called to check what the current map is for that waypoint. If there is a change in map name, then you robot should change its map. + +We don't provide an out-of-the-box hook for this because map switching logic is likely to vary too much between robots and deployments. For example, in a certain deployment you might need the robot to decompose a very large floor into multiple maps internally, but inside the RMF traffic system those multiple maps would all be part of one single map. In that case you'd need a dictionary of nav graph index -> robot's internal map name. + - `waypoints` The sequence of waypoints to follow. When the robot arrives at a waypoint in this sequence, it should wait at that waypoint until the From b605927948fefdcd6c0312c3e673ea44a609901a Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Thu, 29 Sep 2022 19:24:25 +0530 Subject: [PATCH 08/19] solved discussions/145 --- src/integration_fleets_tutorial_cpp.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 500ac98..ca8e530 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -293,6 +293,8 @@ This function will allow you to add a robot to the fleet. Allow this fleet adapter to execute a PerformAction activity of specified category which may be present in sequence event. +> Note:- It's intentionally left up to the system integrators to decide how to structure your implementation when the user might receive different types of perform_action requests. + - `category` A string that categorizes the action. This value should be used when filling out the category field in event_description_PerformAction.json From e13995f86a3bd984c8d3fd71f4425d424303ceb6 Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Fri, 7 Oct 2022 10:40:59 +0530 Subject: [PATCH 09/19] Adds Steps to follow for cpp tutorial Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 316 +++++++++++++++++++++++++ 1 file changed, 316 insertions(+) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index ca8e530..4fd9ff3 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -860,3 +860,319 @@ function as a no-op if your robots do not perform docking procedures. - `docking_finished_callback` Trigger this callback when the docking is finished. + +## Stepst to use rmf_fleet_adapter: + +### Step 1 + +Make an Adapter instance using + +```cpp + static std::shared_ptr make( + const std::string& node_name, + const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions(), + std::optional discovery_timeout = std::nullopt); +``` + +### Step 2 + +Initializing the fleet. + +- First add fleet to adapter + +```cpp + _fleet_handle = adapter->add_fleet( + _fleet_name, *_traits, *_graph, _server_uri); +``` + +- Setting up LaneRequests by setting up some publishers and subscriptions + +```cpp + _closed_lanes_pub = + node->create_publisher( + rmf_fleet_adapter::ClosedLaneTopicName, + rclcpp::SystemDefaultsQoS().reliable().keep_last(1).transient_local()); + + // Create subscription to lane closure requests + _lane_closure_request_sub = + node->create_subscription( + rmf_fleet_adapter::LaneClosureRequestTopicName, + rclcpp::SystemDefaultsQoS(), + [this]( + rmf_fleet_msgs::msg::LaneRequest::UniquePtr request_msg) + { + + if (request_msg->fleet_name != this->_fleet_name && + !request_msg->fleet_name.empty()) + return; + + this->_fleet_handle->open_lanes(request_msg->open_lanes); + this->_fleet_handle->close_lanes(request_msg->close_lanes); + + std::unordered_set newly_closed_lanes; + for (const auto& l : request_msg->close_lanes) + { + if (_closed_lanes.count(l) == 0) + newly_closed_lanes.insert(l); + + _closed_lanes.insert(l); + } + + for (const auto& l : request_msg->open_lanes) + _closed_lanes.erase(l); + + for (auto& [_, robot] : this->_robots) + { + robot->newly_closed_lanes(newly_closed_lanes); + } + + rmf_fleet_msgs::msg::ClosedLanes state_msg; + state_msg.fleet_name = this->_fleet_name; + state_msg.closed_lanes.insert( + state_msg.closed_lanes.begin(), + _closed_lanes.begin(), + _closed_lanes.end()); + + _closed_lanes_pub->publish(state_msg); + }); +``` + +- Initializing some parameters for the fleet (battery, mechanical system, power, etc) + +```cpp + + // Set the fleet state topic publish period + const double fleet_state_frequency = + rmf_fleet["publish_fleet_state"].as(); + _fleet_handle->fleet_state_topic_publish_period( + rmf_traffic::time::from_seconds(1.0/fleet_state_frequency)); + + // Set up parameters required for task planner + // Battery system + const YAML::Node battery = rmf_fleet["battery_system"]; + const double voltage = battery["voltage"].as(); + const double capacity = battery["capacity"].as(); + const double charging_current = battery["charging_current"].as(); + + auto battery_system_optional = rmf_battery::agv::BatterySystem::make( + voltage, capacity, charging_current); + auto battery_system = std::make_shared( + *battery_system_optional); + + // Mechanical system + const YAML::Node mechanical = rmf_fleet["mechanical_system"]; + const double mass = mechanical["mass"].as(); + const double moment_of_inertia = mechanical["moment_of_inertia"].as(); + const double friction = mechanical["friction_coefficient"].as(); + + auto mechanical_system_optional = rmf_battery::agv::MechanicalSystem::make( + mass, moment_of_inertia, friction); + rmf_battery::agv::MechanicalSystem& mechanical_system = + *mechanical_system_optional; + + std::shared_ptr motion_sink = + std::make_shared( + *battery_system, mechanical_system); + + // Ambient power system + const YAML::Node ambient_system = rmf_fleet["ambient_system"]; + const double ambient_power_drain = ambient_system["power"].as(); + auto ambient_power_system = rmf_battery::agv::PowerSystem::make( + ambient_power_drain); + if (!ambient_power_system) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for ambient power system"); + + return false; + } + std::shared_ptr ambient_sink = + std::make_shared( + *battery_system, *ambient_power_system); + + // Tool power system + const YAML::Node tool_system = rmf_fleet["tool_system"]; + const double tool_power_drain = ambient_system["power"].as(); + auto tool_power_system = rmf_battery::agv::PowerSystem::make( + tool_power_drain); + if (!tool_power_system) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for tool power system"); + + return false; + } + std::shared_ptr tool_sink = + std::make_shared( + *battery_system, *tool_power_system); + + // Drain battery + const bool drain_battery = rmf_fleet["account_for_battery_drain"].as(); + // Recharge threshold + const double recharge_threshold = + rmf_fleet["recharge_threshold"].as(); + // Recharge state of charge + const double recharge_soc = rmf_fleet["recharge_soc"].as(); + +``` + +- Setting the fleet's finishing request + +```cpp + + // Finishing tasks + const YAML::Node task_capabilities = rmf_fleet["task_capabilities"]; + const std::string finishing_request_string = + task_capabilities["finishing_request"].as(); + rmf_task::ConstRequestFactoryPtr finishing_request = nullptr; + if (finishing_request_string == "charge") + { + finishing_request = + std::make_shared(); + RCLCPP_INFO( + node->get_logger(), + "Fleet is configured to perform ChargeBattery as finishing request"); + } + else if (finishing_request_string == "park") + { + finishing_request = + std::make_shared(); + RCLCPP_INFO( + node->get_logger(), + "Fleet is configured to perform ParkRobot as finishing request"); + } + else if (finishing_request_string == "nothing") + { + RCLCPP_INFO( + node->get_logger(), + "Fleet is not configured to perform any finishing request"); + } + else + { + RCLCPP_WARN( + node->get_logger(), + "Provided finishing request [%s] is unsupported. The valid " + "finishing requests are [charge, park, nothing]. The task planner will " + " default to [nothing].", + finishing_request_string.c_str()); + } + +``` + +- Adding task capabilities and performable actions to the fleet + +```cpp + // Set task planner params + if (!_fleet_handle->set_task_planner_params( + battery_system, + motion_sink, + ambient_sink, + tool_sink, + recharge_threshold, + recharge_soc, + drain_battery, + finishing_request)) + { + RCLCPP_ERROR( + node->get_logger(), + "Failed to initialize task planner parameters"); + + return false; + } + + // Currently accepting any tasks as long as they are in the config + const auto consider = + [](const nlohmann::json& description, + rmf_fleet_adapter::agv::FleetUpdateHandle::Confirmation& confirm) + { + confirm.accept(); + }; + + const bool perform_loop = task_capabilities["loop"].as(); + if (perform_loop) + { + _fleet_handle->consider_patrol_requests(consider); + } + + const bool perform_delivery = task_capabilities["delivery"].as(); + if (perform_delivery) + { + _fleet_handle->consider_delivery_requests( + consider, consider); + } + + const bool perform_cleaning = task_capabilities["clean"].as(); + if (perform_cleaning) + { + _fleet_handle->consider_cleaning_requests(consider); + } + + // Currently accepting any actions as long as they are in the config + if (task_capabilities["action"]) + { + std::vector action_strings = + task_capabilities["action"].as>(); + for (const auto& action : action_strings) + { + _fleet_handle->add_performable_action(action, consider); + } + } +``` + +- Calculate the Planner::StartSet for each robot using compute_plan_starts with the robot's starting pose + +```cpp +// Use Start or compute plan starts + if (std::holds_alternative(pose)) + { + RCLCPP_INFO(_pimpl->_adapter->node()->get_logger(), + "Using provided Planner::Start to initialize starts " + " for robot %s.", + robot_name.c_str()); + const auto p = std::get(pose); + starts.push_back(p); + } + else if (std::holds_alternative(pose)) + { + RCLCPP_INFO(_pimpl->_adapter->node()->get_logger(), + "Running compute_plan_starts for robot %s.", + robot_name.c_str()); + const auto p = std::get(pose); + + RCLCPP_INFO(_pimpl->_adapter->node()->get_logger(), + "Robot %s pose is [%.2f, %.2f, %.2f]", + robot_name.c_str(), + p.x(), p.y(), p.z()); + + // Use compute plan starts to estimate the start + starts = rmf_traffic::agv::compute_plan_starts( + *_pimpl->_graph, map_name, {p.x(), p.y(), p.z()}, + rmf_traffic_ros2::convert(_pimpl->_adapter->node()->now())); + } + + if (starts.empty()) + { + RCLCPP_ERROR(_pimpl->_adapter->node()->get_logger(), + "Unable to determine StartSet for %s", robot_name.c_str()); + + return false; + } +``` + +- Add each robot to the fleet using FleetUpdateHandle::add_robot() + +```cpp + _pimpl->_fleet_handle->add_robot( + command, robot_name, _pimpl->_traits->profile(), starts, + [w = this->weak_from_this(), command, robot_name = std::move(robot_name)]( + const RobotUpdateHandlePtr& updater) + { + const auto self = w.lock(); + if (!self) + return; + command->set_updater(updater); + self->_pimpl->_robots[robot_name] = command; + }); +``` From 0e3abe308abab2c831050a7afd0357b1e18b2f61 Mon Sep 17 00:00:00 2001 From: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Date: Mon, 10 Oct 2022 13:02:49 +0530 Subject: [PATCH 10/19] Apply suggestions from code review Co-authored-by: Marco A. Gutierrez Signed-off-by: Dev Manek <58616961+thedevmanek@users.noreply.github.com> --- 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 896299f..c9db246 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -1,4 +1,4 @@ -# Fleet Adapter Tutorial +# Fleet Adapter Tutorials `fleet_adapter` acts as a bridge between the robots and the core RMF system. From 848d39ee1deaa755dab48a702449e7662f6a7c4f Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Wed, 12 Oct 2022 16:43:33 +0530 Subject: [PATCH 11/19] Remove API docs and adds gen info about classes Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 859 +------------------------ 1 file changed, 14 insertions(+), 845 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 4fd9ff3..9bcb190 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -1,865 +1,34 @@ # Fleet Adapter C++ Tutorial > The files mentioned in this tutorial can be found [here](https://github.com/open-rmf/rmf_ros2/tree/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv) +> For more information about various functions please visit the hyperlinks in the header files -## [Adapter.hpp](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp) +## [Adapter](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp) -```cpp -#ifndef RMF_FLEET_ADAPTER__AGV__ADAPTER_HPP -#define RMF_FLEET_ADAPTER__AGV__ADAPTER_HPP - -#include -#include - -#include -#include - -#include - -namespace rmf_fleet_adapter { -namespace agv { -class Adapter : public std::enable_shared_from_this -{ -public: - - static std::shared_ptr init_and_make( - const std::string& node_name, - std::optional discovery_timeout = std::nullopt); - - static std::shared_ptr make( - const std::string& node_name, - const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions(), - std::optional discovery_timeout = std::nullopt); - - std::shared_ptr add_fleet( - const std::string& fleet_name, - rmf_traffic::agv::VehicleTraits traits, - rmf_traffic::agv::Graph navigation_graph, - std::optional server_uri = std::nullopt); - - using Blockers = std::vector; - - void add_easy_traffic_light( - std::function handle_callback, - const std::string& fleet_name, - const std::string& robot_name, - rmf_traffic::agv::VehicleTraits traits, - std::function pause_callback, - std::function resume_callback, - std::function deadlock_callback = nullptr); - - - /// Get the rclcpp::Node that this adapter will be using for communication. - std::shared_ptr node(); - - /// const-qualified node() - std::shared_ptr node() const; - - /// Begin running the event loop for this adapter. The event loop will operate - /// in another thread, so this function is non-blocking. - Adapter& start(); - - /// Stop the event loop if it is running. - Adapter& stop(); - - /// Wait until the adapter is done spinning. - /// - /// \sa wait_for() - Adapter& wait(); - - /// Wait until the adapter is done spinning, or until the maximum wait time - /// duration is reached. - /// - /// \sa wait() - Adapter& wait_for(std::chrono::nanoseconds max_wait); - - class Implementation; -private: - Adapter(); - rmf_utils::unique_impl_ptr _pimpl; -}; - -using AdapterPtr = std::shared_ptr; -using ConstAdapterPtr = std::shared_ptr; - -} // namespace agv -} // namespace rmf_fleet_adapter - -#endif // RMF_FLEET_ADAPTER__AGV__ADAPTER_HPP - -``` - -`init_and_make` will initialize rclcpp context and makes an adapter instance.You can add the fleets to be adapted. -takes two arguments - -- node_name - The name of rclcpp node that will be produced for this adapter. -- discovery_timeout - The minimum time it will wait to discover the schedule node before giving up. If the default `rmf_utils:nullopt` is used it will use `discovery_timeout`node parameter or wait for 1 minute if`discovery_timeout` is not defined. - -`make` instantiates an rclcpp::Node like `init_and_make` however it is more customisable. - -- node_name - The name of rclcpp node that will be produced for this adapter. -- node_options - It can be used to customize the node. -- discovery_timeout - The minimum time it will wait to discover the schedule node before giving up. If the default `rmf_utils:nullopt` is used it will use `discovery_timeout` node parameter or wait for 1 minute if `discovery_timeout` is not defined. - -`add_fleet` allows to add a fleet that needs to be adapted. - -- fleet_name - name of the fleet -- traits - specify approximate traits of the vehicle. -- navigation_graph - specify navigation graph used by the vehicles in the fleet -- server_uri - uri of the websocket server that receives updates on the tasks and states. If the default `rmf_utils:nullopt` is used it will not publish any data. - -`add_easy_traffic_light` helps you create simple version of traffic light which allows to manage robot that can only support pause or resume commands.This API is simpler to use than the standard traffic light API but it provides less information about the exact timing needed for the starts and stops.This API must only be used if system integrators can ensure very low-latency and reliable connections to the robots to ensure that commands arrive on time. - -- handle_callback -The callback that will be triggered when the EasyTrafficLight handle is ready to be used. This callback will only be triggered once. -- fleet_name - The name of the fleet -- robot_name -The name of the robot -- traits -The traits of the robot -- pause_callback -The callback that should be triggered by the traffic light when an immediate pause is needed. -- resume_callback -The callback that will be triggered by the traffic light when the robot may resume moving forward. -- deadlock_callbackThe callback that will be triggered by the traffic light if there is a permanent blocker disrupting the ability of this vehicle to proceed.Manual intervention may be required in this circumstance. A callback does not need to be provided for this. Either way, an error message will be printed to the log. - -## [FleetUpdateHandle.hpp](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp) - -The Update Handle works as handler between the fleet robots and the fleetadapter. - -```c++ - -namespace rmf_fleet_adapter { -namespace agv { - -//============================================================================== -class FleetUpdateHandle : public std::enable_shared_from_this -{ -public: - - void add_robot( - std::shared_ptr command, - const std::string& name, - const rmf_traffic::Profile& profile, - rmf_traffic::agv::Plan::StartSet start, - std::function handle)> handle_cb); - - /// Confirmation is a class used by the task acceptance callbacks to decide if - /// a task description should be accepted. - class Confirmation - { - public: - - /// Constructor - Confirmation(); - - /// Call this function to decide that you want to accept the task request. - /// If this function is never called, it will be assumed that the task is - /// rejected. - Confirmation& accept(); - - /// Check whether - bool is_accepted() const; - - /// Call this function to bring attention to errors related to the task - /// request. Each call to this function will overwrite any past calls, so - /// it is recommended to only call it once. - Confirmation& errors(std::vector error_messages); - - /// Call this function to add errors instead of overwriting the ones that - /// were already there. - Confirmation& add_errors(std::vector error_messages); - - /// Check the errors that have been given to this confirmation. - const std::vector& errors() const; - - class Implementation; - private: - rmf_utils::impl_ptr _pimpl; - }; - - using ConsiderRequest = - std::function; - - FleetUpdateHandle& consider_delivery_requests( - ConsiderRequest consider_pickup, - ConsiderRequest consider_dropoff); - - - FleetUpdateHandle& consider_cleaning_requests(ConsiderRequest consider); - - FleetUpdateHandle& consider_patrol_requests(ConsiderRequest consider); +The `Adapter` class provides functions using which new robot fleets can be added.The class provides two major functions one is `add_fleet` which allows the user to add a new fleet and second is `add_easy_traffic_light` which allows the user to add a easy traffic light system. - FleetUpdateHandle& consider_composed_requests(ConsiderRequest consider); - - - FleetUpdateHandle& add_performable_action( - const std::string& category, - ConsiderRequest consider); - - /// Specify a set of lanes that should be closed. - void close_lanes(std::vector lane_indices); - - /// Specify a set of lanes that should be open. - void open_lanes(std::vector lane_indices); - - - bool set_task_planner_params( - std::shared_ptr battery_system, - std::shared_ptr motion_sink, - std::shared_ptr ambient_sink, - std::shared_ptr tool_sink, - double recharge_threshold, - double recharge_soc, - bool account_for_battery_drain, - rmf_task::ConstRequestFactoryPtr finishing_requst = nullptr); - - using AcceptTaskRequest = - std::function; - - using AcceptDeliveryRequest = - std::function; - - /// Specify the default value for how high the delay of the current itinerary - /// can become before it gets interrupted and replanned. A nullopt value will - /// allow for an arbitrarily long delay to build up without being interrupted. - FleetUpdateHandle& default_maximum_delay( - std::optional value); - - /// Get the default value for the maximum acceptable delay. - std::optional default_maximum_delay() const; - - /// The behavior is identical to fleet_state_topic_publish_period - [[deprecated("Use fleet_state_topic_publish_period instead")]] - FleetUpdateHandle& fleet_state_publish_period( - std::optional value); - - /// Specify a period for how often the fleet state message is published for - /// this fleet. Passing in std::nullopt will disable the fleet state message - /// publishing. The default value is 1s. - FleetUpdateHandle& fleet_state_topic_publish_period( - std::optional value); - - /// Specify a period for how often the fleet state is updated in the database - /// and to the API server. This is separate from publishing the fleet state - /// over the ROS2 fleet state topic. Passing in std::nullopt will disable - /// the updating, but this is not recommended unless you intend to provide the - /// API server with the fleet states through some other means. - /// - /// The default value is 1s. - FleetUpdateHandle& fleet_state_update_period( - std::optional value); - - /// Set a callback for listening to update messages (e.g. fleet states and - /// task updates). This will not receive any update messages that happened - /// before the listener was set. - FleetUpdateHandle& set_update_listener( - std::function listener); - - // Do not allow moving - FleetUpdateHandle(FleetUpdateHandle&&) = delete; - FleetUpdateHandle& operator=(FleetUpdateHandle&&) = delete; - - class Implementation; -private: - FleetUpdateHandle(); - rmf_utils::unique_impl_ptr _pimpl; -}; - -using FleetUpdateHandlePtr = std::shared_ptr; -using ConstFleetUpdateHandlePtr = std::shared_ptr; - -} // namespace agv -} // namespace rmf_fleet_adapter - -#endif // RMF_FLEET_ADAPTER__AGV__FLEETUPDATEHANDLE_HPP - -``` - -`add_robot` -This function will allow you to add a robot to the fleet. - -> Note currently there is no support for deleting the robot - -- `command` - A reference to a command handle for this robot. -- `name` - The name of this robot. -- `profile` - The profile of this robot. This profile should account for the largest possible payload that the robot might carry. -- `start` The initial location of the robot, expressed as `Plan::StartSet`. Multiple Start objects might be needed if the robot is not starting precisely on a waypoint.The function `rmf_traffic::agv::compute_plan_starts()` may help with this. -- `handle_cb` - This callback function will get triggered when the RobotUpdateHandle is ready to be used by the Fleet API side of the Adapter. Setting up a new robot requires communication with the Schedule Node, so there may be a delay before the robot is ready to be used. - -`add_performable_action` -Allow this fleet adapter to execute a PerformAction activity of specified -category which may be present in sequence event. +## [FleetUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp) > Note:- It's intentionally left up to the system integrators to decide how to structure your implementation when the user might receive different types of perform_action requests. +> Currently there is no support for deleting the robot. -- `category` - A string that categorizes the action. This value should be used when - filling out the category field in event_description_PerformAction.json - schema. -- `consider` - Decide whether to accept the action based on the description field in - event_description_PerformAction.json schema. - -`set_task_planner_params` -Set the parameters required for task planning. Without calling this -function, this fleet will not bid for and accept tasks. - -- `battery_system` - Specify the battery system used by the vehicles in this fleet. -- `motion_sink` - Specify the motion sink that describes the vehicles in this fleet. -- `ambient_sink` - Specify the device sink for ambient sensors used by the vehicles in this fleet. -- `tool_sink` - Specify the device sink for special tools used by the vehicles in this fleet. -- `recharge_threshold` - The threshold for state of charge below which robots in this fleet - will cease to operate and require recharging. A value between 0.0 and - 1.0 should be specified. - When the robot is idle, a timer periodically computes the estimated battery drain if the robot were to head back to the charger now. If the battery level after the estimated drain is within a safety factor applied to the recharge_threshold, the TaskManager will automatically begin a task to take the robot back to its charging station. - The recharge_threshold is used in a similar manner during task allocation. When deciding the order of tasks performed, the TaskPlanner will estimate the battery level of the robot after performing a task A. If the estimate is below the recharge_threshold, it will then check if the robot can perform task A if it performed a recharge task before this. If this is the case then it will automatically include a recharge task before task A. If it still cannot perform task A even after recharging, it will discard the possibility of doing task A in that sequence. -- `recharge_soc` - The state of charge to which robots in this fleet should be charged up - to by automatic recharging tasks. A value between 0.0 and 1.0 should be - specified. -- `account_for_battery_drain` - Specify whether battery drain is to be considered while allocating tasks. - If false, battery drain will not be considered when planning for tasks. - As a consequence, charging tasks will not be automatically assigned to - vehicles in this fleet when battery levels fall below the - recharge_threshold. -- `finishing_request` - A factory for a request that should be performed by each robot in this - fleet at the end of its assignments. - -[`RobotUpdateHandle.hpp`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) -The `RobotUpdateHandle` helps in sending updates to the robot. - -```cpp -namespace rmf_fleet_adapter { -namespace agv { - -//============================================================================== -/// You will be given an instance of this class every time you add a new robot -/// to your fleet. Use that instance to send updates to RoMi-H about your -/// robot's state. -class RobotUpdateHandle -{ -public: +The Update Handle works as handler between the fleet robots and the fleetadapter.The work of the handler is to act +`add_robot` allows user to add robot to the fleet.Users can use `FleetUpdateHandle` to allow the Adapter to consider requests like delivery,cleaning,patrol etc.More information is present in the API documentation.Users can also provide set of open or closed lanes. - [[deprecated("Use replan() instead")]] - void interrupted(); +### Information regarding charging of the robot +When the robot is idle, a timer periodically computes the estimated battery drain if the robot were to head back to the charger now. If the battery level after the estimated drain is within a safety factor applied to the recharge_threshold, the TaskManager will automatically begin a task to take the robot back to its charging station. +The recharge_threshold is used in a similar manner during task allocation. When deciding the order of tasks performed, the TaskPlanner will estimate the battery level of the robot after performing a task A. If the estimate is below the recharge_threshold, it will then check if the robot can perform task A if it performed a recharge task before this. If this is the case then it will automatically include a recharge task before task A. If it still cannot perform task A even after recharging, it will discard the possibility of doing task A in that sequence. - void replan(); +## [RobotUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) - void update_position( - std::size_t waypoint, - double orientation); - - void update_position( - const Eigen::Vector3d& position, - const std::vector& lanes); - - void update_position( - const Eigen::Vector3d& position, - std::size_t target_waypoint); - - void update_position( - const std::string& map_name, - const Eigen::Vector3d& position, - const double max_merge_waypoint_distance = 0.1, - const double max_merge_lane_distance = 1.0, - const double min_lane_length = 1e-8); - - void update_position(rmf_traffic::agv::Plan::StartSet position); - - RobotUpdateHandle& set_charger_waypoint(const std::size_t charger_wp); - - void update_battery_soc(const double battery_soc); - - void override_status(std::optional status); - - RobotUpdateHandle& maximum_delay( - rmf_utils::optional value); - - - rmf_utils::optional maximum_delay() const; - - /// The ActionExecution class should be used to manage the execution of and - /// provide updates on ongoing actions. - class ActionExecution - { - public: - /// Update the amount of time remaining for this action - void update_remaining_time(rmf_traffic::Duration remaining_time_estimate); - - void underway(std::optional text); - - void error(std::optional text); - - - void delayed(std::optional text); - - - void blocked(std::optional text); - - void finished(); - - bool okay() const; - - class Implementation; - private: - ActionExecution(); - rmf_utils::impl_ptr _pimpl; - }; - - using ActionExecutor = std::function; - - /// Set the ActionExecutor for this robot - void set_action_executor(ActionExecutor action_executor); - - - void submit_direct_request( - nlohmann::json task_request, - std::string request_id, - std::function receive_response); - - /// An object to maintain an interruption of the current task. When this - /// object is destroyed, the task will resume. - class Interruption - { - public: - /// Call this function to resume the task while providing labels for - /// resuming. - void resume(std::vector labels); +The `RobotUpdateHandle` helps in sending updates to the robot.An instance of this class will be given everytime a new robot is added to the fleet.Users can use this instance of the class to send updates to RoMi-H about the robot's state. - class Implementation; - private: - Interruption(); - rmf_utils::unique_impl_ptr _pimpl; - }; +## [RobotCommandHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotCommandHandle.hpp) - Interruption interrupt( - std::vector labels, - std::function robot_is_interrupted); - - - void cancel_task( - std::string task_id, - std::vector labels, - std::function on_cancellation); - - void kill_task( - std::string task_id, - std::vector labels, - std::function on_kill); - - enum class Tier - { - /// General status information, does not require special attention - Info, - - /// Something unusual that might require attention - Warning, - - /// A critical failure that requires immediate operator attention - Error - }; - - /// An object to maintain an issue that is happening with the robot. When this - /// object is destroyed without calling resolve(), the issue will be - /// "dropped", which issues a warning to the log. - class IssueTicket - { - public: - - /// Indicate that the issue has been resolved. The provided message will be - /// logged for this robot and the issue will be removed from the robot - /// state. - void resolve(nlohmann::json msg); - - class Implementation; - private: - IssueTicket(); - rmf_utils::unique_impl_ptr _pimpl; - }; - - - IssueTicket create_issue( - Tier tier, std::string category, nlohmann::json detail); - - // TODO(MXG): Should we offer a "clear_all_issues" function? - - /// Add a log entry with Info severity - void log_info(std::string text); - - /// Add a log entry with Warning severity - void log_warning(std::string text); - - /// Add a log entry with Error severity - void log_error(std::string text); - - /// Toggle the responsive wait behavior for this robot. When responsive wait - /// is active, the robot will remain in the traffic schedule when it is idle - /// and will negotiate its position with other traffic participants to - /// potentially move out of their way. - /// - /// Disabling this behavior may be helpful to reduce CPU load or prevent - /// parked robots from moving or being seen as conflicts when they are not - /// actually at risk of creating traffic conflicts. - /// - /// By default this behavior is enabled. - void enable_responsive_wait(bool value); - - class Implementation; - - /// This API is experimental and will not be supported in the future. Users - /// are to avoid relying on these feature for any integration. - class Unstable - { - public: - /// True if this robot is allowed to accept new tasks. False if the robot - /// will not accept any new tasks. - bool is_commissioned() const; - - /// Stop this robot from accepting any new tasks. It will continue to - /// perform tasks that are already in its queue. To reassign those tasks, - /// you will need to use the task request API to cancel the tasks and - /// re-request them. - void decommission(); - - /// Allow this robot to resume accepting new tasks. - void recommission(); - - /// Get the schedule participant of this robot - rmf_traffic::schedule::Participant* get_participant(); - - /// Change the radius of the footprint and vicinity of this participant. - void change_participant_profile( - double footprint_radius, - double vicinity_radius); - - /// Override the schedule to say that the robot will be holding at a certain - /// position. This should not be used while tasks with automatic schedule - /// updating are running, or else the traffic schedule will have jumbled up - /// information, which can be disruptive to the overall traffic management. - void declare_holding( - std::string on_map, - Eigen::Vector3d at_position, - rmf_traffic::Duration for_duration = std::chrono::seconds(30)); - - /// Get the current Plan ID that this robot has sent to the traffic schedule - rmf_traffic::PlanId current_plan_id() const; - - /// Hold onto this class to tell the robot to behave as a "stubborn - /// negotiator", meaning it will always refuse to accommodate the schedule - /// of any other agent. This could be used when teleoperating a robot, to - /// tell other robots that the agent is unable to negotiate. - /// - /// When the object is destroyed, the stubbornness will automatically be - /// released. - class Stubbornness - { - public: - /// Stop being stubborn - void release(); - - class Implementation; - private: - Stubbornness(); - rmf_utils::impl_ptr _pimpl; - }; - - /// Tell this robot to be a stubborn negotiator. - Stubbornness be_stubborn(); - - enum class Decision - { - Undefined = 0, - Clear = 1, - Crowded = 2 - }; - - /// A callback with this signature will be given to the watchdog when the - /// robot is ready to enter a lift. If the watchdog passes in a true, then - /// the robot will proceed to enter the lift. If the watchdog passes in a - /// false, then the fleet adapter will release its session with the lift and - /// resume later. - using Decide = std::function; - - using Watchdog = std::function; - - /// Set a callback that can be used to check whether the robot is clear to - /// enter the lift. - void set_lift_entry_watchdog( - Watchdog watchdog, - rmf_traffic::Duration wait_duration = std::chrono::seconds(10)); - - private: - friend Implementation; - Implementation* _pimpl; - }; - - /// Get a mutable reference to the unstable API extension - Unstable& unstable(); - /// Get a const reference to the unstable API extension - const Unstable& unstable() const; - -private: - RobotUpdateHandle(); - rmf_utils::unique_impl_ptr _pimpl; -}; - -using RobotUpdateHandlePtr = std::shared_ptr; -using ConstRobotUpdateHandlePtr = std::shared_ptr; - -} // namespace agv -} // namespace rmf_fleet_adapter - - - -``` - -`update_position` -This function updates the position of the robot by specifying -`waypoint`-The waypoint the robot is on. - -- `orientation` - Current orientation of the robot.' - - or - -- `position` -position of the robot and one or more lanes that the robot is occupying. -- `lanes` - number of lanes(minimum 1 required) - - or - -- `position` -position of the robot and one or more lanes that the robot is occupying. -- `target_waypoint` - number of lanes(minimum 1 required) - This should be used if the robot has diverged significantly from its - course but it is merging back onto a waypoint. - - or - -- `map_name` - The map robot is on -- `position` -position of the robot and one or more lanes that the robot is occupying. - This option should be used if the robot has diverged significantly from its course. - The parameters for this function are passed along to `rmf_traffic::agv::compute_plan_starts()`. - - or - -- `position` The plan set for the robot - -`replan` -The function will tell the RMF schedule that the robot requires a new plan. The new plan will have the starting position of the robot as the last position the robot had updated using `update_position()`. - -`set_charger_waypoint` -Set the waypoint where the charger for this robot is located.If not specified, the nearest waypoint in the graph with the is_charger() -property will be assumed as the charger for this robot. - -- `charger_wp` The waypoint where the charger for this robot is located. - -`update_battery_soc` -Update the current battery level of the robot by specifying its state of charge as a fraction of its total charge capacity. - -`override_status` This function overrides the robot status. - -- `status`The string provided must - be a valid enum as specified in the robot_state.json schema. - Pass std::nullopt to cancel the override and allow RMF to automatically - update the status. The default value is std::nullopt. - -`maximum_delay` -Specify how high the delay of the current itinerary can become before it gets interrupted and replanned. A nullopt value will allow for an -arbitrarily long delay to build up without being interrupted. - -`ActionExecution` - -- `underways` - Set task status to error and optionally log a message (error tier) - -- `errors` - Set the task status to delayed and optionally log a message - (warning tier) -- `delayeds` - Set the task status to blocked and optionally log a message - (warning tier) -- `blockeds` - Trigger this when the action is successfully finished -- `finished` - Returns false if the Action has been killed or cancelled - -`submit_direct_request` - -- `task_request` A JSON description of the task request. It should match the task_request.json schema of rmf_api_msgs, in particular it must contain -- `request_id` The unique ID for this task request. -- `receive_response` Provide a callback to receive the response. The response will be a robot_task_response.json message from rmf_api_msgs (note: this message - -`ActionExecutor` -Signature for a callback to request the robot to perform an action - -- `category` - A category of the action to be performed - -- `description` - A description of the action to be performed - -- `execution` - An ActionExecution object that will be provided to the user for - updating the state of the action. - -`set_action_executor` Set the ActionExecutor for this robot - -`submit_direct_request` Send a request to the robot. - -- `task_request` The json task request -- `request_id` The request ID for the task request -- `receive_response` The response callback - -`interrupt` Interrupt (pause) the current task, yielding control of the robot away -from the fleet adapter's task manager. - -- `labels` - Labels that will be assigned to this interruption. It is recommended to - include information about why the interruption is happening. - -`cancel_task` -Cancel a task, if it has been assigned to this robot - -- `task_id` - The ID of the task to be canceled - -- `labels` - Labels that will be assigned to this cancellation. It is recommended to - include information about why the cancellation is happening. - -- `on_cancellation` - Callback that will be triggered after the cancellation is issued. - task_was_found will be true if the task was successfully found and - issued the cancellation, false otherwise. - -`kill_task` Kill a task, if it has been assigned to this robot - -- `task_id` - The ID of the task to be canceled - -- `labels` - Labels that will be assigned to this cancellation. It is recommended to - include information about why the cancellation is happening. - -- `on_kill` - Callback that will be triggered after the cancellation is issued. - task_was_found will be true if the task was successfully found and - issued the kill, false otherwise. - -`create_issue` Create a new issue for the robot. - -- `tier` - The severity of the issue - -- `category` - A brief category to describe the issue - -- `detail` - Full details of the issue that might be relevant to an operator or - logging system. - -[`RobotCommandHandle`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotCommandHandle.hpp) This class works as a bridge and acts as a handler for all the commands that need to be sent to the robot. -```cpp -namespace rmf_fleet_adapter { -namespace agv { - -//============================================================================== -/// Implement this class to receive robot commands from RMF -class RobotCommandHandle -{ -public: - - using Duration = rmf_traffic::Duration; - - using ArrivalEstimator = - std::function; - - /// Trigger this callback function when the follow_new_path request has been - /// completed. It should only be triggered that one time and then discarded. - using RequestCompleted = std::function; - - - virtual void follow_new_path( - const std::vector& waypoints, - ArrivalEstimator next_arrival_estimator, - RequestCompleted path_finished_callback) = 0; - - /// Have the robot come to an immediate stop. - virtual void stop() = 0; - - - virtual void dock( - const std::string& dock_name, - RequestCompleted docking_finished_callback) = 0; - - // Virtual destructor - virtual ~RobotCommandHandle() = default; -}; - -} // namespace agv -} // namespace rmf_fleet_adapter - -``` - -`ArrivalEstimator` -Use this callback function to update the fleet adapter on how long the -robot will take to reach its next destination. - -- `path_index` - The index of the path element that the robot is currently heading - towards. - -- `remaining_time` - An estimate of how much longer the robot will take to arrive at - `path_index`. - -`follow_new_path` Have the robot follow a new path. If it was already following a path, then -it should immediately switch over to this one. - -> Note:- The waypoints argument that will be given to the follow_new_path function of the RobotCommandHandle contains a list of `rmf_traffic::agv::Plan::Waypoint` objects that have a graph_index function.When that graph_index is non-null it can be passed to `Graph::get_waypoint` and then `Graph::Waypoint::get_map_name` can be called to check what the current map is for that waypoint. If there is a change in map name, then you robot should change its map. - -We don't provide an out-of-the-box hook for this because map switching logic is likely to vary too much between robots and deployments. For example, in a certain deployment you might need the robot to decompose a very large floor into multiple maps internally, but inside the RMF traffic system those multiple maps would all be part of one single map. In that case you'd need a dictionary of nav graph index -> robot's internal map name. - -- `waypoints` - The sequence of waypoints to follow. When the robot arrives at a - waypoint in this sequence, it should wait at that waypoint until the - clock reaches the time() field of the waypoint. This is important - because the waypoint timing is used to avoid traffic conflicts with - other vehicles. - -- `next_arrival_estimator` - Use this callback to give estimates for how long the robot will take to - reach the path element of the specified index. You should still be - calling RobotUpdateHandle::update_position() even as you call this - function. - -- `path_finished_callback` - Trigger this callback when the robot is done following the new path. - -You do not need to trigger waypoint_arrival_callback when triggering -this one. - -`dock` Have the robot begin a pre-defined docking procedure. Implement this -function as a no-op if your robots do not perform docking procedures. - -- `dock_name` - The predefined name of the docking procedure to use. - -- `docking_finished_callback` - Trigger this callback when the docking is finished. +> Note:- The waypoints argument that will be given to the follow_new_path function of the RobotCommandHandle contains a list of `rmf_traffic::agv::Plan::Waypoint` objects that have a graph_index function.When that graph_index is non-null it can be passed to `Graph::get_waypoint` and then `Graph::Waypoint::get_map_name` can be called to check what the current map is for that waypoint. If there is a change in map name, then you robot should change its map.We don't provide an out-of-the-box hook for this because map switching logic is likely to vary too much between robots and deployments. For example, in a certain deployment you might need the robot to decompose a very large floor into multiple maps internally, but inside the RMF traffic system those multiple maps would all be part of one single map. In that case you'd need a dictionary of nav graph index -> robot's internal map name. ## Stepst to use rmf_fleet_adapter: From 7893139be9051da384c2eeb5eb03c6e09c23ee40 Mon Sep 17 00:00:00 2001 From: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Date: Fri, 14 Oct 2022 14:03:18 +0530 Subject: [PATCH 12/19] Apply suggestions from code review Co-authored-by: Xiyu Co-authored-by: Marco A. Gutierrez Signed-off-by: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 258 ++++--------------------- 1 file changed, 37 insertions(+), 221 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 9bcb190..bc402b1 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -5,15 +5,14 @@ ## [Adapter](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp) -The `Adapter` class provides functions using which new robot fleets can be added.The class provides two major functions one is `add_fleet` which allows the user to add a new fleet and second is `add_easy_traffic_light` which allows the user to add a easy traffic light system. +The `Adapter` class helps you to communicate your fleet with other core RMF systems. You may use the function `add_fleet` to register your fleet with RMF. ## [FleetUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp) -> Note:- It's intentionally left up to the system integrators to decide how to structure your implementation when the user might receive different types of perform_action requests. +> Note: It's intentionally left up to the system integrators to decide how to structure your implementation when the user might receive different types of perform_action requests. > Currently there is no support for deleting the robot. -The Update Handle works as handler between the fleet robots and the fleetadapter.The work of the handler is to act -`add_robot` allows user to add robot to the fleet.Users can use `FleetUpdateHandle` to allow the Adapter to consider requests like delivery,cleaning,patrol etc.More information is present in the API documentation.Users can also provide set of open or closed lanes. +The `FleetUpdateHandle` works as handler between the fleet adapter and RMF. It tells RMF what are the tasks, requests and actions that the fleet can accept. Users can use the `FleetUpdateHandle` to register their robots to the fleet using `add_robot`, as well as add task capabilities (e.g. delivery, patrol, cleaning) and performable actions so that RMF can delegate them to the appropriate fleets accordingly. More information can be found in the API documentation. ### Information regarding charging of the robot @@ -22,25 +21,25 @@ The recharge_threshold is used in a similar manner during task allocation. When ## [RobotUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) -The `RobotUpdateHandle` helps in sending updates to the robot.An instance of this class will be given everytime a new robot is added to the fleet.Users can use this instance of the class to send updates to RoMi-H about the robot's state. +The `RobotUpdateHandle` contains important callbacks to keep RMF updated about the robot's status. ## [RobotCommandHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotCommandHandle.hpp) -This class works as a bridge and acts as a handler for all the commands that need to be sent to the robot. +This is an abstract class that users have to implement in order for their fleet adapters to receive commands from RMF successfully. Users should implement the functions `follow_new_path`, `stop` and `dock`, then fill in the internal logic accordingly to relay the navigation commands to their robot. > Note:- The waypoints argument that will be given to the follow_new_path function of the RobotCommandHandle contains a list of `rmf_traffic::agv::Plan::Waypoint` objects that have a graph_index function.When that graph_index is non-null it can be passed to `Graph::get_waypoint` and then `Graph::Waypoint::get_map_name` can be called to check what the current map is for that waypoint. If there is a change in map name, then you robot should change its map.We don't provide an out-of-the-box hook for this because map switching logic is likely to vary too much between robots and deployments. For example, in a certain deployment you might need the robot to decompose a very large floor into multiple maps internally, but inside the RMF traffic system those multiple maps would all be part of one single map. In that case you'd need a dictionary of nav graph index -> robot's internal map name. -## Stepst to use rmf_fleet_adapter: +## Steps to use rmf_fleet_adapter: ### Step 1 Make an Adapter instance using ```cpp - static std::shared_ptr make( - const std::string& node_name, - const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions(), - std::optional discovery_timeout = std::nullopt); +static std::shared_ptr make( +const std::string& node_name, +const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions(), +std::optional discovery_timeout = std::nullopt); ``` ### Step 2 @@ -106,242 +105,59 @@ Initializing the fleet. }); ``` -- Initializing some parameters for the fleet (battery, mechanical system, power, etc) +- Setting important parameters for the task planner. Users can create the respective objects for these parameters and pass them to the fleet handle via `set_task_planner_param`. The header files for these objects can be found in the [`rmf_battery`](https://github.com/open-rmf/rmf_battery/tree/main/rmf_battery/include/rmf_battery/agv) repository. +For example, to set the battery system parameters: ```cpp - - // Set the fleet state topic publish period - const double fleet_state_frequency = - rmf_fleet["publish_fleet_state"].as(); - _fleet_handle->fleet_state_topic_publish_period( - rmf_traffic::time::from_seconds(1.0/fleet_state_frequency)); - - // Set up parameters required for task planner - // Battery system - const YAML::Node battery = rmf_fleet["battery_system"]; - const double voltage = battery["voltage"].as(); - const double capacity = battery["capacity"].as(); - const double charging_current = battery["charging_current"].as(); - - auto battery_system_optional = rmf_battery::agv::BatterySystem::make( +auto battery_system_optional = rmf_battery::agv::BatterySystem::make( voltage, capacity, charging_current); - auto battery_system = std::make_shared( +auto battery_system = std::make_shared( *battery_system_optional); - - // Mechanical system - const YAML::Node mechanical = rmf_fleet["mechanical_system"]; - const double mass = mechanical["mass"].as(); - const double moment_of_inertia = mechanical["moment_of_inertia"].as(); - const double friction = mechanical["friction_coefficient"].as(); - - auto mechanical_system_optional = rmf_battery::agv::MechanicalSystem::make( - mass, moment_of_inertia, friction); - rmf_battery::agv::MechanicalSystem& mechanical_system = - *mechanical_system_optional; - - std::shared_ptr motion_sink = - std::make_shared( - *battery_system, mechanical_system); - - // Ambient power system - const YAML::Node ambient_system = rmf_fleet["ambient_system"]; - const double ambient_power_drain = ambient_system["power"].as(); - auto ambient_power_system = rmf_battery::agv::PowerSystem::make( - ambient_power_drain); - if (!ambient_power_system) - { - RCLCPP_ERROR( - node->get_logger(), - "Invalid values supplied for ambient power system"); - - return false; - } - std::shared_ptr ambient_sink = - std::make_shared( - *battery_system, *ambient_power_system); - - // Tool power system - const YAML::Node tool_system = rmf_fleet["tool_system"]; - const double tool_power_drain = ambient_system["power"].as(); - auto tool_power_system = rmf_battery::agv::PowerSystem::make( - tool_power_drain); - if (!tool_power_system) - { - RCLCPP_ERROR( - node->get_logger(), - "Invalid values supplied for tool power system"); - - return false; - } - std::shared_ptr tool_sink = - std::make_shared( - *battery_system, *tool_power_system); - - // Drain battery - const bool drain_battery = rmf_fleet["account_for_battery_drain"].as(); - // Recharge threshold - const double recharge_threshold = - rmf_fleet["recharge_threshold"].as(); - // Recharge state of charge - const double recharge_soc = rmf_fleet["recharge_soc"].as(); - ``` - Setting the fleet's finishing request +You may choose a finishing task for your fleet's robots between `park`, `charge` and `nothing`. You will need to create a `rmf_task::ConstRequestFactoryPtr` object and set them to the right finishing request. +The header files for the finishing requests can be found [here](https://github.com/open-rmf/rmf_task/tree/main/rmf_task/include/rmf_task/requests) in the `rmf_task` repository. -```cpp - - // Finishing tasks - const YAML::Node task_capabilities = rmf_fleet["task_capabilities"]; - const std::string finishing_request_string = - task_capabilities["finishing_request"].as(); - rmf_task::ConstRequestFactoryPtr finishing_request = nullptr; - if (finishing_request_string == "charge") - { - finishing_request = - std::make_shared(); - RCLCPP_INFO( - node->get_logger(), - "Fleet is configured to perform ChargeBattery as finishing request"); - } - else if (finishing_request_string == "park") - { - finishing_request = - std::make_shared(); - RCLCPP_INFO( - node->get_logger(), - "Fleet is configured to perform ParkRobot as finishing request"); - } - else if (finishing_request_string == "nothing") - { - RCLCPP_INFO( - node->get_logger(), - "Fleet is not configured to perform any finishing request"); - } - else - { - RCLCPP_WARN( - node->get_logger(), - "Provided finishing request [%s] is unsupported. The valid " - "finishing requests are [charge, park, nothing]. The task planner will " - " default to [nothing].", - finishing_request_string.c_str()); - } +For example, to set the finishing request to `charge`: +```cpp +rmf_task::ConstRequestFactoryPtr finishing_request = nullptr; +finishing_request = std::make_shared(); ``` - Adding task capabilities and performable actions to the fleet +Use the `FleetUpdateHandle` to add task capabilities and performable actions to your fleet. For example, if your fleet is able to carry out patrol tasks and teleop actions, you can add them as such: ```cpp - // Set task planner params - if (!_fleet_handle->set_task_planner_params( - battery_system, - motion_sink, - ambient_sink, - tool_sink, - recharge_threshold, - recharge_soc, - drain_battery, - finishing_request)) - { - RCLCPP_ERROR( - node->get_logger(), - "Failed to initialize task planner parameters"); - - return false; - } - - // Currently accepting any tasks as long as they are in the config - const auto consider = +const auto consider = [](const nlohmann::json& description, rmf_fleet_adapter::agv::FleetUpdateHandle::Confirmation& confirm) { confirm.accept(); }; - - const bool perform_loop = task_capabilities["loop"].as(); - if (perform_loop) - { - _fleet_handle->consider_patrol_requests(consider); - } - - const bool perform_delivery = task_capabilities["delivery"].as(); - if (perform_delivery) - { - _fleet_handle->consider_delivery_requests( - consider, consider); - } - - const bool perform_cleaning = task_capabilities["clean"].as(); - if (perform_cleaning) - { - _fleet_handle->consider_cleaning_requests(consider); - } - - // Currently accepting any actions as long as they are in the config - if (task_capabilities["action"]) - { - std::vector action_strings = - task_capabilities["action"].as>(); - for (const auto& action : action_strings) - { - _fleet_handle->add_performable_action(action, consider); - } - } +_fleet_handle->consider_patrol_requests(consider); +_fleet_handle->add_performable_action("teleop", consider); ``` -- Calculate the Planner::StartSet for each robot using compute_plan_starts with the robot's starting pose +### Step 3 + +- Compute `Planner::StartSet` for each robot +The fleet adapter will require a `Planner::StartSet` to start planning for each robot. This object can be obtained via an `Eigen::Vector3d` of the robot's starting pose using [`rmf_traffic::agv::compute_plan_starts`](https://github.com/open-rmf/rmf_traffic/blob/main/rmf_traffic/include/rmf_traffic/agv/Planner.hpp#L916-L923): ```cpp -// Use Start or compute plan starts - if (std::holds_alternative(pose)) - { - RCLCPP_INFO(_pimpl->_adapter->node()->get_logger(), - "Using provided Planner::Start to initialize starts " - " for robot %s.", - robot_name.c_str()); - const auto p = std::get(pose); - starts.push_back(p); - } - else if (std::holds_alternative(pose)) - { - RCLCPP_INFO(_pimpl->_adapter->node()->get_logger(), - "Running compute_plan_starts for robot %s.", - robot_name.c_str()); - const auto p = std::get(pose); - - RCLCPP_INFO(_pimpl->_adapter->node()->get_logger(), - "Robot %s pose is [%.2f, %.2f, %.2f]", - robot_name.c_str(), - p.x(), p.y(), p.z()); - - // Use compute plan starts to estimate the start - starts = rmf_traffic::agv::compute_plan_starts( - *_pimpl->_graph, map_name, {p.x(), p.y(), p.z()}, - rmf_traffic_ros2::convert(_pimpl->_adapter->node()->now())); - } - - if (starts.empty()) - { - RCLCPP_ERROR(_pimpl->_adapter->node()->get_logger(), - "Unable to determine StartSet for %s", robot_name.c_str()); - - return false; - } +starts = rmf_traffic::agv::compute_plan_starts( + *_graph, map_name, {p.x(), p.y(), p.z()}, + rmf_traffic_ros2::convert(_adapter->node()->now())); ``` -- Add each robot to the fleet using FleetUpdateHandle::add_robot() +- Add each robot to the fleet using [`FleetUpdateHandle::add_robot`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp#L71-L76) ```cpp - _pimpl->_fleet_handle->add_robot( - command, robot_name, _pimpl->_traits->profile(), starts, - [w = this->weak_from_this(), command, robot_name = std::move(robot_name)]( - const RobotUpdateHandlePtr& updater) - { - const auto self = w.lock(); - if (!self) - return; +_fleet_handle->add_robot( + command, robot_name, _traits->profile(), starts, + [command](const RobotUpdateHandlePtr& updater) + { command->set_updater(updater); - self->_pimpl->_robots[robot_name] = command; - }); + }); ``` From 90722ceaf2576f16293768c932c4507ba02ea68b Mon Sep 17 00:00:00 2001 From: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Date: Fri, 14 Oct 2022 14:07:17 +0530 Subject: [PATCH 13/19] Fixed the code block issue Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index bc402b1..6403437 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -126,8 +126,19 @@ rmf_task::ConstRequestFactoryPtr finishing_request = nullptr; finishing_request = std::make_shared(); ``` -- Adding task capabilities and performable actions to the fleet - +Users can now set the initialized task planner parameters with the fleet handle using `FleetUpdateHandle::set_task_planner_params`. +```cpp +_fleet_handle->set_task_planner_params( + battery_system, + motion_sink, + ambient_sink, + tool_sink, + recharge_threshold, + recharge_soc, + account_for_battery_drain, + finishing_request +) +``` Use the `FleetUpdateHandle` to add task capabilities and performable actions to your fleet. For example, if your fleet is able to carry out patrol tasks and teleop actions, you can add them as such: ```cpp const auto consider = From bb531c78f24636ead76ba2ea2a05c5b8d92e21f4 Mon Sep 17 00:00:00 2001 From: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Date: Fri, 14 Oct 2022 14:12:06 +0530 Subject: [PATCH 14/19] Update src/integration_fleets_tutorial_cpp.md Co-authored-by: Xiyu Signed-off-by: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 6403437..5fe6581 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -37,9 +37,9 @@ Make an Adapter instance using ```cpp static std::shared_ptr make( -const std::string& node_name, -const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions(), -std::optional discovery_timeout = std::nullopt); + const std::string& node_name, + const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions(), + std::optional discovery_timeout = std::nullopt); ``` ### Step 2 From 8d18f874484834bce58e655a5846e5d68eb35370 Mon Sep 17 00:00:00 2001 From: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Date: Fri, 14 Oct 2022 14:16:43 +0530 Subject: [PATCH 15/19] Update src/integration_fleets_tutorial.md Co-authored-by: Marco A. Gutierrez Signed-off-by: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Signed-off-by: Dev Manek --- 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 c9db246..6dbf4e3 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -18,7 +18,7 @@ The `fleet_adapter` receives information (position, current ongoing tasks, batte - 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 fleet adapter will then send the navigation commands to the robot using the appropriate API. ## Tutorials From 8d9e698dd2045740ab1d7bef92d30928af1921ad Mon Sep 17 00:00:00 2001 From: Dev Manek Date: Fri, 14 Oct 2022 15:33:16 +0530 Subject: [PATCH 16/19] Adds question to the QnA parts Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 44 ++++++++++++++++++++------ 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 5fe6581..c076161 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -9,15 +9,34 @@ The `Adapter` class helps you to communicate your fleet with other core RMF syst ## [FleetUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp) -> Note: It's intentionally left up to the system integrators to decide how to structure your implementation when the user might receive different types of perform_action requests. > Currently there is no support for deleting the robot. +- Question: [How do I create multiple PerformActions?](https://github.com/open-rmf/rmf/discussions/145) + + It's intentionally left up to the system integrators to decide how to structure their implementation when the user might receive different types of `perform_action` requests. + A simple example for creating multiple PerformActions can be found below + + ```cpp + using ActionExecutor = rmf_fleet_adapter::agv::RobotUpdateHnadle::ActionExecutor; + std::shared_ptr>> executor_map; + ActionExecutor dispatching_executor = [executor_map]( + const std::string& category, + const nlohmann::json& description, + ActionExecution execution) + { + executor_map->at(category)(category, description, std::move(execution)); + }; + robot_update_handle->set_action_executor(dispatching_executor); + ``` + + `executor_map` can be given a different executor for each `perform_action` category that is expected. + The `FleetUpdateHandle` works as handler between the fleet adapter and RMF. It tells RMF what are the tasks, requests and actions that the fleet can accept. Users can use the `FleetUpdateHandle` to register their robots to the fleet using `add_robot`, as well as add task capabilities (e.g. delivery, patrol, cleaning) and performable actions so that RMF can delegate them to the appropriate fleets accordingly. More information can be found in the API documentation. -### Information regarding charging of the robot +- Question: [How does the automatic go to battery task work?](https://github.com/open-rmf/rmf/discussions/184) -When the robot is idle, a timer periodically computes the estimated battery drain if the robot were to head back to the charger now. If the battery level after the estimated drain is within a safety factor applied to the recharge_threshold, the TaskManager will automatically begin a task to take the robot back to its charging station. -The recharge_threshold is used in a similar manner during task allocation. When deciding the order of tasks performed, the TaskPlanner will estimate the battery level of the robot after performing a task A. If the estimate is below the recharge_threshold, it will then check if the robot can perform task A if it performed a recharge task before this. If this is the case then it will automatically include a recharge task before task A. If it still cannot perform task A even after recharging, it will discard the possibility of doing task A in that sequence. + When the robot is idle, a timer periodically computes the estimated battery drain if the robot were to head back to the charger now. If the battery level after the estimated drain is within a safety factor applied to the `recharge_threshold`, the `TaskManager` will automatically begin a task to take the robot back to its charging station. + The `recharge_threshold` is used in a similar manner during task allocation. When deciding the order of tasks performed, the `TaskPlanner` will estimate the battery level of the robot after performing a _task A_. If the estimate is below the `recharge_threshold`, it will then check if the robot can perform _task A_ if it performed a recharge task before this. If this is the case then it will automatically include a recharge task before task A. If it still cannot perform _task A_ even after recharging, it will discard the possibility of doing _task A_ in that sequence. ## [RobotUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) @@ -27,7 +46,9 @@ The `RobotUpdateHandle` contains important callbacks to keep RMF updated about t This is an abstract class that users have to implement in order for their fleet adapters to receive commands from RMF successfully. Users should implement the functions `follow_new_path`, `stop` and `dock`, then fill in the internal logic accordingly to relay the navigation commands to their robot. -> Note:- The waypoints argument that will be given to the follow_new_path function of the RobotCommandHandle contains a list of `rmf_traffic::agv::Plan::Waypoint` objects that have a graph_index function.When that graph_index is non-null it can be passed to `Graph::get_waypoint` and then `Graph::Waypoint::get_map_name` can be called to check what the current map is for that waypoint. If there is a change in map name, then you robot should change its map.We don't provide an out-of-the-box hook for this because map switching logic is likely to vary too much between robots and deployments. For example, in a certain deployment you might need the robot to decompose a very large floor into multiple maps internally, but inside the RMF traffic system those multiple maps would all be part of one single map. In that case you'd need a dictionary of nav graph index -> robot's internal map name. +- Question: [How do I tell the fleet adapter if the map has changed?](https://github.com/open-rmf/rmf/discussions/180) + + The waypoints argument that will be given to the `follow_new_path` function of the `RobotCommandHandle` contains a list of `rmf_traffic::agv::Plan::Waypoint` objects that have a `graph_index` function.When that `graph_index` is non-null it can be passed to `Graph::get_waypoint` and then `Graph::Waypoint::get_map_name` can be called to check what the current map is for that waypoint. If there is a change in map name, then you robot should change its map.We don't provide an out-of-the-box hook for this because map switching logic is likely to vary too much between robots and deployments. For example, in a certain deployment you might need the robot to decompose a very large floor into multiple maps internally, but inside the RMF traffic system those multiple maps would all be part of one single map. In that case you'd need a dictionary of nav graph index -> robot's internal map name. ## Steps to use rmf_fleet_adapter: @@ -105,8 +126,8 @@ Initializing the fleet. }); ``` -- Setting important parameters for the task planner. Users can create the respective objects for these parameters and pass them to the fleet handle via `set_task_planner_param`. The header files for these objects can be found in the [`rmf_battery`](https://github.com/open-rmf/rmf_battery/tree/main/rmf_battery/include/rmf_battery/agv) repository. -For example, to set the battery system parameters: +- Setting important parameters for the task planner. Users can create the respective objects for these parameters and pass them to the fleet handle via `set_task_planner_param`. The header files for these objects can be found in the [`rmf_battery`](https://github.com/open-rmf/rmf_battery/tree/main/rmf_battery/include/rmf_battery/agv) repository. + For example, to set the battery system parameters: ```cpp auto battery_system_optional = rmf_battery::agv::BatterySystem::make( @@ -116,8 +137,8 @@ auto battery_system = std::make_shared( ``` - Setting the fleet's finishing request -You may choose a finishing task for your fleet's robots between `park`, `charge` and `nothing`. You will need to create a `rmf_task::ConstRequestFactoryPtr` object and set them to the right finishing request. -The header files for the finishing requests can be found [here](https://github.com/open-rmf/rmf_task/tree/main/rmf_task/include/rmf_task/requests) in the `rmf_task` repository. + You may choose a finishing task for your fleet's robots between `park`, `charge` and `nothing`. You will need to create a `rmf_task::ConstRequestFactoryPtr` object and set them to the right finishing request. + The header files for the finishing requests can be found [here](https://github.com/open-rmf/rmf_task/tree/main/rmf_task/include/rmf_task/requests) in the `rmf_task` repository. For example, to set the finishing request to `charge`: @@ -127,6 +148,7 @@ finishing_request = std::make_shared() ``` Users can now set the initialized task planner parameters with the fleet handle using `FleetUpdateHandle::set_task_planner_params`. + ```cpp _fleet_handle->set_task_planner_params( battery_system, @@ -139,7 +161,9 @@ _fleet_handle->set_task_planner_params( finishing_request ) ``` + Use the `FleetUpdateHandle` to add task capabilities and performable actions to your fleet. For example, if your fleet is able to carry out patrol tasks and teleop actions, you can add them as such: + ```cpp const auto consider = [](const nlohmann::json& description, @@ -154,7 +178,7 @@ _fleet_handle->add_performable_action("teleop", consider); ### Step 3 - Compute `Planner::StartSet` for each robot -The fleet adapter will require a `Planner::StartSet` to start planning for each robot. This object can be obtained via an `Eigen::Vector3d` of the robot's starting pose using [`rmf_traffic::agv::compute_plan_starts`](https://github.com/open-rmf/rmf_traffic/blob/main/rmf_traffic/include/rmf_traffic/agv/Planner.hpp#L916-L923): + The fleet adapter will require a `Planner::StartSet` to start planning for each robot. This object can be obtained via an `Eigen::Vector3d` of the robot's starting pose using [`rmf_traffic::agv::compute_plan_starts`](https://github.com/open-rmf/rmf_traffic/blob/main/rmf_traffic/include/rmf_traffic/agv/Planner.hpp#L916-L923): ```cpp starts = rmf_traffic::agv::compute_plan_starts( From 695a6447f79789ff1ba6311d479f995847b3b496 Mon Sep 17 00:00:00 2001 From: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Date: Fri, 14 Oct 2022 22:35:41 +0530 Subject: [PATCH 17/19] Update integration_fleets_tutorial_cpp.md Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 46 +++++++++++++------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index c076161..a3b1141 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -11,32 +11,9 @@ The `Adapter` class helps you to communicate your fleet with other core RMF syst > Currently there is no support for deleting the robot. -- Question: [How do I create multiple PerformActions?](https://github.com/open-rmf/rmf/discussions/145) - - It's intentionally left up to the system integrators to decide how to structure their implementation when the user might receive different types of `perform_action` requests. - A simple example for creating multiple PerformActions can be found below - - ```cpp - using ActionExecutor = rmf_fleet_adapter::agv::RobotUpdateHnadle::ActionExecutor; - std::shared_ptr>> executor_map; - ActionExecutor dispatching_executor = [executor_map]( - const std::string& category, - const nlohmann::json& description, - ActionExecution execution) - { - executor_map->at(category)(category, description, std::move(execution)); - }; - robot_update_handle->set_action_executor(dispatching_executor); - ``` - - `executor_map` can be given a different executor for each `perform_action` category that is expected. The `FleetUpdateHandle` works as handler between the fleet adapter and RMF. It tells RMF what are the tasks, requests and actions that the fleet can accept. Users can use the `FleetUpdateHandle` to register their robots to the fleet using `add_robot`, as well as add task capabilities (e.g. delivery, patrol, cleaning) and performable actions so that RMF can delegate them to the appropriate fleets accordingly. More information can be found in the API documentation. -- Question: [How does the automatic go to battery task work?](https://github.com/open-rmf/rmf/discussions/184) - - When the robot is idle, a timer periodically computes the estimated battery drain if the robot were to head back to the charger now. If the battery level after the estimated drain is within a safety factor applied to the `recharge_threshold`, the `TaskManager` will automatically begin a task to take the robot back to its charging station. - The `recharge_threshold` is used in a similar manner during task allocation. When deciding the order of tasks performed, the `TaskPlanner` will estimate the battery level of the robot after performing a _task A_. If the estimate is below the `recharge_threshold`, it will then check if the robot can perform _task A_ if it performed a recharge task before this. If this is the case then it will automatically include a recharge task before task A. If it still cannot perform _task A_ even after recharging, it will discard the possibility of doing _task A_ in that sequence. ## [RobotUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) @@ -135,6 +112,10 @@ auto battery_system_optional = rmf_battery::agv::BatterySystem::make( auto battery_system = std::make_shared( *battery_system_optional); ``` +- Question: [How does the automatic go to battery task work?](https://github.com/open-rmf/rmf/discussions/184) + + When the robot is idle, a timer periodically computes the estimated battery drain if the robot were to head back to the charger now. If the battery level after the estimated drain is within a safety factor applied to the `recharge_threshold`, the `TaskManager` will automatically begin a task to take the robot back to its charging station. + The `recharge_threshold` is used in a similar manner during task allocation. When deciding the order of tasks performed, the `TaskPlanner` will estimate the battery level of the robot after performing a _task A_. If the estimate is below the `recharge_threshold`, it will then check if the robot can perform _task A_ if it performed a recharge task before this. If this is the case then it will automatically include a recharge task before task A. If it still cannot perform _task A_ even after recharging, it will discard the possibility of doing _task A_ in that sequence. - Setting the fleet's finishing request You may choose a finishing task for your fleet's robots between `park`, `charge` and `nothing`. You will need to create a `rmf_task::ConstRequestFactoryPtr` object and set them to the right finishing request. @@ -174,6 +155,25 @@ const auto consider = _fleet_handle->consider_patrol_requests(consider); _fleet_handle->add_performable_action("teleop", consider); ``` +- Question: [How do I create multiple PerformActions?](https://github.com/open-rmf/rmf/discussions/145) + + It's intentionally left up to the system integrators to decide how to structure their implementation when the user might receive different types of `perform_action` requests. + A simple example for creating multiple PerformActions can be found below + + ```cpp + using ActionExecutor = rmf_fleet_adapter::agv::RobotUpdateHnadle::ActionExecutor; + std::shared_ptr>> executor_map; + ActionExecutor dispatching_executor = [executor_map]( + const std::string& category, + const nlohmann::json& description, + ActionExecution execution) + { + executor_map->at(category)(category, description, std::move(execution)); + }; + robot_update_handle->set_action_executor(dispatching_executor); + ``` + + `executor_map` can be given a different executor for each `perform_action` category that is expected. ### Step 3 From c707a3fbf667ab3680eeeaf03bf7fd08e32bf2bc Mon Sep 17 00:00:00 2001 From: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Date: Fri, 14 Oct 2022 22:36:31 +0530 Subject: [PATCH 18/19] Update src/integration_fleets_tutorial_cpp.md Co-authored-by: Xiyu Signed-off-by: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 1 - 1 file changed, 1 deletion(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index a3b1141..1adb493 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -9,7 +9,6 @@ The `Adapter` class helps you to communicate your fleet with other core RMF syst ## [FleetUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp) -> Currently there is no support for deleting the robot. The `FleetUpdateHandle` works as handler between the fleet adapter and RMF. It tells RMF what are the tasks, requests and actions that the fleet can accept. Users can use the `FleetUpdateHandle` to register their robots to the fleet using `add_robot`, as well as add task capabilities (e.g. delivery, patrol, cleaning) and performable actions so that RMF can delegate them to the appropriate fleets accordingly. More information can be found in the API documentation. From c58048ba8d0cfe735d82180721b55876f50e156a Mon Sep 17 00:00:00 2001 From: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Date: Fri, 14 Oct 2022 23:01:53 +0530 Subject: [PATCH 19/19] Update src/integration_fleets_tutorial_cpp.md Co-authored-by: Xiyu Signed-off-by: Dev Manek <58616961+thedevmanek@users.noreply.github.com> Signed-off-by: Dev Manek --- src/integration_fleets_tutorial_cpp.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/integration_fleets_tutorial_cpp.md b/src/integration_fleets_tutorial_cpp.md index 1adb493..3b6cd3a 100644 --- a/src/integration_fleets_tutorial_cpp.md +++ b/src/integration_fleets_tutorial_cpp.md @@ -186,7 +186,7 @@ starts = rmf_traffic::agv::compute_plan_starts( ``` - Add each robot to the fleet using [`FleetUpdateHandle::add_robot`](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp#L71-L76) - +> Currently there is no support for deleting the robot. ```cpp _fleet_handle->add_robot( command, robot_name, _traits->profile(), starts,