Open-RMF Fleet Adapter (#34)

* adapter

* Update links

* Update repo main readme

* Move docstring before code in utils.py

* Minor code style changes

* New line at end of file

* Changed link to pacakge

* Update README.md

* Update version number in setup.py

* Contributing and license section

* Updated instructions for this repos docker setup

README.md

@@ -8,6 +8,11 @@ standards, with a focus on AMRs (Autonomous Mobile Robots).
 
 The following packages are included in this repository:
 
+### Full Control Fleet Adapter for RMF and InOrbit
+
+The [rmf_inorbit_fleet_adapter](https://github.com/inorbit-ai/ros_amr_interop/tree/humble-devel/rmf_inorbit_fleet_adapter) package contains a Full Control [Open-RMF](https://github.com/open-rmf/rmf#robotics-middleware-framework-rmf) Fleet Adapter that allows RMF to control a fleet of autonomous robots through the InOrbit API.
+For demonstrations of this adapter or a template to configure your own fleet, visit the the InOrbit RMF [Fleet Adapter Examples](https://github.com/inorbit-ai/rmf_inorbit_examples) repository.
+
 ### VDA5050 Connector for ROS2
 
 The [vda5050_connector](https://github.com/inorbit-ai/ros_amr_interop/tree/galactic-devel/vda5050_connector#readme)
@@ -53,6 +58,7 @@ Install [pre-commit](https://pre-commit.com/) in your computer and then set it u
 
 | Package | Foxy Source | Foxy Debian | Galactic Source | Galactic Debian |
 | :---: | :---: | :---: | :---: | :---: |
+| rmf_inorbit_fleet_adapter | N/A | N/A | N/A | N/A |
 | vda5050_connector | N/A | N/A | [![Build Status](http://build.ros2.org/job/Gsrc_uF__vda5050_connector__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__vda5050_connector__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__vda5050_connector__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__vda5050_connector__ubuntu_focal_amd64__binary/) |
 | vda5050_msgs | N/A | N/A | [![Build Status](http://build.ros2.org/job/Gsrc_uF__vda5050_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__vda5050_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__vda5050_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__vda5050_msgs__ubuntu_focal_amd64__binary/) |
 | vda5050_serializer | N/A | N/A | [![Build Status](http://build.ros2.org/job/Gsrc_uF__vda5050_serializer__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__vda5050_serializer__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__vda5050_serializer__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__vda5050_serializer__ubuntu_focal_amd64__binary/) |

rmf_inorbit_fleet_adapter/CONTRIBUTING

@@ -0,0 +1,3 @@
+Any contribution that you make to this repository will
+be under the 3-Clause BSD License, as dictated by that
+[license](https://opensource.org/licenses/BSD-3-Clause).

rmf_inorbit_fleet_adapter/LICENSE

@@ -0,0 +1,11 @@
+Copyright 2023 InOrbit, Inc.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

rmf_inorbit_fleet_adapter/README.md

@@ -0,0 +1,211 @@
+![InOrbit + Open-RMF](assets/open%20rmf%20inorbit%20github%20header%20narrow%202.png)
+
+Full Control Fleet Adapter for integrating InOrbit with [Robotics Middleware Framework](https://github.com/open-rmf/rmf#robotics-middleware-framework-rmf) (RMF).
+
+## Overview
+
+The package includes the RMF InOrbit Fleet Adapter, which enables communication between a fleet of robots controlled by InOrbit and RMF core, allowing centralized coordinated control of a fleet of multiple robots. It utilizes the InOrbit REST API to communicate with InOrbit, while the adapter and RMF run on ROS2 in different nodes.
+Demos and a template can be found in the [`rmf_inorbit_examples`](https://github.com/inorbit-ai/rmf_inorbit_examples) repository, containing a usage demonstration of the InOrbit fleet adapter in a simulated environment and a base configuration to be modified for a particular scenario.
+Each instance of this adapter will work for a fleet in one location. For multiple locations, like offices in different buildings, different instances of the adapter have to be configured and run independently. For each adapter modify the [template package](https://github.com/inorbit-ai/rmf_inorbit_examples/tree/main/rmf_inorbit_template) following the instructions in it.
+
+![mission video](assets/full%20mission.gif)
+
+## Features
+
+This package allows sending tasks to a heterogeneous fleet of robots with the same capabilities in the same location controlled via InOrbit. It supports loop tasks for demonstration purposes and as a proof of concept.
+It does not currently include the following functionality:
+
+- Dispatching InOrbit actions such as docking
+- Tasks other than Loops, such as cleaning or delivery tasks
+
+## Environment
+
+ROS2 Humble + Ubuntu 22.04 / Docker
+
+## Setup
+
+To utilize the fleet adapter an InOrbit account must be set up first. To run a demo simulation, take a look at the documentation of the [`rmf_inorbit_demos`](https://github.com/inorbit-ai/rmf_inorbit_examples/tree/main/rmf_inorbit_demos) package at [`rmf_inorbit_examples`](https://github.com/inorbit-ai/rmf_inorbit_examples) for instructions, and to use the fleet adapter in your own environment, visit [`rmf_inorbit_template`](https://github.com/inorbit-ai/rmf_inorbit_examples/tree/main/rmf_inorbit_template) in the same repository.
+
+## How to use it
+
+The package is prepared to be run in a dockerized environment, but it can also be run on a non docker box.
+
+### Workspace setup
+
+Create the workspace directory tree in your host machine before running the container:
+
+```
+mkdir -p inorbit_rmf_ws/src
+cd inorbit_rmf_ws/src
+```
+
+#### Build and run the docker environment
+
+Run the following script to build the docker image and run a container:
+
+```
+cd .ci/docker
+./start_local_dev.sh
+```
+
+Install dependencies:
+
+```
+# Required:
+python3 -m pip install requests
+
+# If you want to be able to edit traffic maps:
+sudo apt update && sudo apt install -y \
+    ros-humble-rmf-traffic-editor \
+    ros-humble-rmf-building-map-tools
+```
+
+#### Non docker setup
+
+If you don't want to use docker, ignore the previous two steps and:
+
+1. In a Ubuntu 22.04 box, install ROS2 Humble following these [instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html).
+2. Install dependencies:
+
+```
+# Required:
+python3 -m pip install requests
+
+# If you want to be able to edit traffic maps:
+sudo apt update && sudo apt install -y \
+    ros-humble-rmf-traffic-editor \
+    ros-humble-rmf-building-map-tools
+```
+
+3. Source ROS2 Humble installation
+
+```
+source /opt/ros/humble/setup.bash
+```
+
+### Project build
+
+To build the packages, inside the container (at `~/ws`) run:
+
+```
+rosdep install --from-paths src --rosdistro humble -y --ignore-src
+colcon build
+```
+
+A warning about the deprecation of `setup.py` will pop up. This is a known ROS issue, and you can choose to ignore this warning by adding the following environment variable:
+
+```
+echo 'PYTHONWARNINGS="ignore:setup.py install is deprecated::setuptools.command.install"; export PYTHONWARNINGS' >> ~/.bashrc
+```
+
+### Test
+
+After building your package, you can run tests by:
+
+```
+colcon test
+colcon test-result
+```
+
+## Run the adapter
+
+Remember to source the overlay on every new bash session:
+
+```
+. install/setup.bash
+```
+
+The demos and template packages include examples of launch files for the adapter and RMF, as well as instructions for how to set up the adapter configuration.
+The fleet adapter package contains one launch file for the adapter alone, that takes the following arguments:
+
+```
+$ ros2 launch rmf_inorbit_fleet_adapter rmf_inorbit_fleet_adapter.launch.xml --show-args
+Arguments (pass arguments as '<name>:=<value>'):
+
+    'api_key':
+        InOrbit API key
+
+    'adapter_config_file':
+        Path to the configuration file of the adapter
+
+    'nav_graph_file':
+        Path to the navigation graph file for RMF
+
+```
+
+To launch just the adapter (note that it will eventually stop if the underlying infrastructure is not running):
+
+```
+ros2 launch rmf_inorbit_fleet_adapter rmf_inorbit_fleet_adapter.launch.xml api_key:=<InOrbit API KEY> adapter_config_file:=<path to configuration file> nav_graph_file:=<path to navigation graph file>
+```
+
+It will start the following:
+
+#### Nodes
+
+- `/{fleet name}_fleet_adapter`: Instance of the full control fleet adapter node rmf_adapter provides.
+- `/inorbit_fleet_command_handle`: Implementation of rmf_adapter.RobotCommandHandle
+
+All of the topics and services are part of the mentioned nodes. For more information about them, visit [rmf_ros2](https://github.com/open-rmf/rmf_ros2).
+
+#### Topics
+
+```
+/dispenser_requests
+/dispenser_results
+/dispenser_states
+/dock_summary
+/fleet_states
+/ingestor_requests
+/ingestor_results
+/ingestor_states
+/lane_states
+/nav_graphs
+/rmf_traffic/query_update_1
+/task_summaries
+```
+
+#### Services
+
+```
+/<InOrbitSite>_fleet_adapter/describe_parameters
+/<InOrbitSite>_fleet_adapter/get_parameter_types
+/<InOrbitSite>_fleet_adapter/get_parameters
+/<InOrbitSite>_fleet_adapter/list_parameters
+/<InOrbitSite>_fleet_adapter/set_parameters
+/<InOrbitSite>_fleet_adapter/set_parameters_atomically
+/inorbit_fleet_command_handle/describe_parameters
+/inorbit_fleet_command_handle/get_parameter_types
+/inorbit_fleet_command_handle/get_parameters
+/inorbit_fleet_command_handle/list_parameters
+/inorbit_fleet_command_handle/set_parameters
+/inorbit_fleet_command_handle/set_parameters_atomically
+```
+
+### Parameters
+
+```
+/<InOrbitSite>_fleet_adapter:
+    discovery_timeout
+    qos_overrides./parameter_events.publisher.depth
+    qos_overrides./parameter_events.publisher.durability
+    qos_overrides./parameter_events.publisher.history
+    qos_overrides./parameter_events.publisher.reliability
+    use_sim_time
+/inorbit_fleet_command_handle:
+    use_sim_time
+```
+
+## Configuration
+
+Visit [`rmf_inorbit_examples`](https://github.com/inorbit-ai/rmf_inorbit_examples) to get access to the demos and the template configuration package.
+
+## Contributing
+
+Please see the [CONTRIBUTING](CONTRIBUTING.md) document.
+
+## License
+
+[![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg)](LICENSE)
+
+![Powered by InOrbit](assets/open%20rmf%20inorbit%20github%20footer.png)

rmf_inorbit_fleet_adapter/launch/rmf_inorbit_fleet_adapter.launch.xml

@@ -0,0 +1,12 @@
+<?xml version='1.0' ?>
+
+<!-- Copyright 2023 InOrbit, Inc. -->
+<!-- Launches the InOrbit RMF fleet adapter node -->
+
+<launch>
+  <arg name="api_key" description="InOrbit API key"/>
+  <arg name="adapter_config_file" description="Path to the configuration file of the adapter"/>
+  <arg name="nav_graph_file" description="Path to the navigation graph file for RMF"/>
+
+  <node pkg="rmf_inorbit_fleet_adapter" exec="fleet_adapter" args="-c $(var adapter_config_file) -n $(var nav_graph_file) -k $(var api_key)"/>
+</launch>

rmf_inorbit_fleet_adapter/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rmf_inorbit_fleet_adapter</name>
+  <version>0.1.0</version>
+  <description>Full Control Fleet Adapter for integrating InOrbit with Open-RMF</description>
+  <author email="[email protected]">Julian Cerruti</author>
+  <author email="[email protected]">Tomás Badenes</author>
+  <maintainer email="[email protected]">Julian Cerruti</maintainer>
+  <url type="repository">https://github.com/inorbit-ai/ros_amr_interop</url>
+  <license file="LICENSE">3-Clause BSD License</license>
+
+  <build_depend>python3-pip</build_depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-schema</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+
+  <exec_depend>rmf_fleet_adapter_python</exec_depend>
+  <exec_depend>rmf_task_msgs</exec_depend>
+  <exec_depend>rmf_traffic_ros2</exec_depend>
+  <exec_depend>rmf_visualization</exec_depend>
+  <exec_depend>rmf_task_ros2</exec_depend>
+
+  <!--
+    Missing packages are installed on docker build
+    If building and running without docker, install missing packages by running
+      sudo apt install python3-pip
+      python3 -m pip requests
+  -->
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/Requester.py

@@ -0,0 +1,66 @@
+# Copyright 2023 InOrbit, Inc.
+
+import requests
+from requests import Response
+from rclpy.impl.rcutils_logger import RcutilsLogger
+
+
+class Requester:
+    """Generates requests to InOrbit REST API"""
+
+    def __init__(self, base_url: str, robot_id: str, headers: dict, logger: RcutilsLogger) -> None:
+        """
+        Args:
+            base_url (str): Base URL of the InOrbit API
+            robot_id (str): ID of the robot the requests will be made to
+            headers (dict): Request headers
+            logger (RcutilsLogger): Node logger
+        """
+
+        assert base_url, "Base URL is empty"
+        assert robot_id, "No robot_id provided"
+        assert isinstance(headers, dict), "Incorrect header format"
+        assert logger, "Logger not provided"
+
+        self.base_url = base_url
+        self.robot_id = robot_id
+        self.headers = headers
+        self.logger = logger
+
+    def robot_get_request(self, endpoint: str, json=None) -> Response | None:
+        """
+        Makes a GET request to '{BASE_URL}{robot_id}{endpoint}' with the class defined headers and returns the response if successful or None if an error occurred
+
+        Args:
+            endpoint (str): With leading forward slash
+        """
+
+        url = f"{self.base_url}{self.robot_id}{endpoint}"
+        try:
+            res = requests.get(url, headers=self.headers, json=json)
+            if res.status_code >= 300:
+                self.logger.warn(
+                    f"\nStatus code {res.status_code} GETting {url} with body {json}\nmessage: {res.text}")
+            return res
+        except Exception as e:
+            self.logger.error(f"Exception GETting {url}: {e}")
+        return None
+
+    def robot_post_request(self, endpoint: str, json=None) -> Response | None:
+        """
+        Makes a POST request to '{BASE_URL}{robot_id}{endpoint}' with the class defined headers and returns the response if successful or None if an error occurred
+
+        Args:
+            endpoint (str): With leading forward slash
+        """
+
+        url = f"{self.base_url}{self.robot_id}{endpoint}"
+        try:
+            res = requests.post(url, headers=self.headers, json=json)
+            if res.status_code >= 300:
+                self.logger.warn(
+                    f"\nStatus code {res.status_code} POSTing {url} with body {json}\nmessage: {res.text}")
+            return res
+        except Exception as e:
+            self.logger.error(f"Exception POSTing {url}: {e}")
+        return None