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..6dbf4e3 100644 --- a/src/integration_fleets_tutorial.md +++ b/src/integration_fleets_tutorial.md @@ -1,4 +1,4 @@ -# Fleet Adapter Tutorial (Python) +# Fleet Adapter Tutorials `fleet_adapter` acts as a bridge between the robots and the core RMF system. @@ -18,325 +18,9 @@ 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. -> 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..3b6cd3a --- /dev/null +++ b/src/integration_fleets_tutorial_cpp.md @@ -0,0 +1,197 @@ +# 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](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp) + +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) + + + +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. + + +## [RobotUpdateHandle](https://github.com/open-rmf/rmf_ros2/blob/main/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp) + +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 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. + +- 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: + +### 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); + }); +``` + +- 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( + voltage, capacity, charging_current); +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. + 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`: + +```cpp +rmf_task::ConstRequestFactoryPtr finishing_request = nullptr; +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, + 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 = + [](const nlohmann::json& description, + rmf_fleet_adapter::agv::FleetUpdateHandle::Confirmation& confirm) + { + confirm.accept(); + }; +_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 + +- 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 +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`](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, + [command](const RobotUpdateHandlePtr& updater) + { + command->set_updater(updater); + }); +``` 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