diff --git a/README.md b/README.md index e8f977e..a395026 100644 --- a/README.md +++ b/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/) | diff --git a/rmf_inorbit_fleet_adapter/CONTRIBUTING b/rmf_inorbit_fleet_adapter/CONTRIBUTING new file mode 100644 index 0000000..309be1e --- /dev/null +++ b/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). diff --git a/rmf_inorbit_fleet_adapter/LICENSE b/rmf_inorbit_fleet_adapter/LICENSE new file mode 100644 index 0000000..0608c19 --- /dev/null +++ b/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. diff --git a/rmf_inorbit_fleet_adapter/README.md b/rmf_inorbit_fleet_adapter/README.md new file mode 100644 index 0000000..16f3d01 --- /dev/null +++ b/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 ':='): + + '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:= adapter_config_file:= nav_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 + +``` +/_fleet_adapter/describe_parameters +/_fleet_adapter/get_parameter_types +/_fleet_adapter/get_parameters +/_fleet_adapter/list_parameters +/_fleet_adapter/set_parameters +/_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 + +``` +/_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) diff --git a/rmf_inorbit_fleet_adapter/assets/full mission.gif b/rmf_inorbit_fleet_adapter/assets/full mission.gif new file mode 100644 index 0000000..bc89929 Binary files /dev/null and b/rmf_inorbit_fleet_adapter/assets/full mission.gif differ diff --git a/rmf_inorbit_fleet_adapter/assets/open rmf inorbit github footer.png b/rmf_inorbit_fleet_adapter/assets/open rmf inorbit github footer.png new file mode 100644 index 0000000..899b99f Binary files /dev/null and b/rmf_inorbit_fleet_adapter/assets/open rmf inorbit github footer.png differ diff --git a/rmf_inorbit_fleet_adapter/assets/open rmf inorbit github header narrow 2.png b/rmf_inorbit_fleet_adapter/assets/open rmf inorbit github header narrow 2.png new file mode 100644 index 0000000..88ca4aa Binary files /dev/null and b/rmf_inorbit_fleet_adapter/assets/open rmf inorbit github header narrow 2.png differ diff --git a/rmf_inorbit_fleet_adapter/launch/rmf_inorbit_fleet_adapter.launch.xml b/rmf_inorbit_fleet_adapter/launch/rmf_inorbit_fleet_adapter.launch.xml new file mode 100644 index 0000000..3d33626 --- /dev/null +++ b/rmf_inorbit_fleet_adapter/launch/rmf_inorbit_fleet_adapter.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/rmf_inorbit_fleet_adapter/package.xml b/rmf_inorbit_fleet_adapter/package.xml new file mode 100644 index 0000000..55e8b05 --- /dev/null +++ b/rmf_inorbit_fleet_adapter/package.xml @@ -0,0 +1,36 @@ + + + + rmf_inorbit_fleet_adapter + 0.1.0 + Full Control Fleet Adapter for integrating InOrbit with Open-RMF + Julian Cerruti + Tomás Badenes + Julian Cerruti + https://github.com/inorbit-ai/ros_amr_interop + 3-Clause BSD License + + python3-pip + + python3-numpy + python3-schema + ros2launch + rclpy + + rmf_fleet_adapter_python + rmf_task_msgs + rmf_traffic_ros2 + rmf_visualization + rmf_task_ros2 + + + + + ament_python + + diff --git a/rmf_inorbit_fleet_adapter/resource/rmf_inorbit_fleet_adapter b/rmf_inorbit_fleet_adapter/resource/rmf_inorbit_fleet_adapter new file mode 100644 index 0000000..e69de29 diff --git a/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/Requester.py b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/Requester.py new file mode 100644 index 0000000..449f019 --- /dev/null +++ b/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 diff --git a/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/RobotClientAPI.py b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/RobotClientAPI.py new file mode 100644 index 0000000..fbb7dcd --- /dev/null +++ b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/RobotClientAPI.py @@ -0,0 +1,240 @@ +# Copyright 2023 InOrbit, Inc. +# +# This file has been copied and adapted from +# https://github.com/open-rmf/fleet_adapter_template/blob/32f47a451ea67fa299a4fdb1a189d5db8df84b6b/fleet_adapter_template/fleet_adapter_template/RobotClientAPI.py +# +# The following is a summary of the introduced changes: +# - Implemented or modified all methods +# - Modified the intended use of the class to one instance per robot +# +# The following is a copy of the original license note: +# +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +from schema import Schema, Optional + +from rclpy.impl.rcutils_logger import RcutilsLogger +from .Requester import Requester + +from .utils import euc_distance, angle_diff + + +class RobotAPI: + """ + The RobotAPI class is a wrapper for API calls to the robot. Here are implemented the + functions which will be used by the RobotCommandHandle. + """ + + API_KEY_HEADER = "x-auth-inorbit-app-key" + STOP_COMMAND_PAYLOAD = {'actionId': 'CancelNavGoal-000000'} + + CONFIG_SCHEMA = Schema({ + "max_delay": float, + "robot_id": str, + "battery_charge_attribute_id": str, + "charging_status_attribute_id": str, + Optional("actions", default={}): { # Empty yaml dicts are parsed as None. Default actions to empty dict + Optional("dock", default=None): { + "action_id": str, + "dock_name": str, + } + } + }) + + def __init__(self, api_key: str, name: str, robot_config: dict, max_lin_speed: float, logger: RcutilsLogger, base_url="https://api.inorbit.ai/robots/", threshold_distance=0.22, threshold_angle=0.2): + """ + Takes the parameters required to submit requests to InOrbitAPI + + Args: + api_key (str): InOrbit API KEY + name (str): Robot name as appears in the adapter config. It is taken with the purpose of providing more descriptive logging + robot_config (dict): Robot config set in the adapter configuration. The required keys are: + max_delay (float): Allowed seconds of delay of the current itinerary before it gets interrupted and replanned (will not affect RobotAPI) + robot_id (str): InOrbit robot id + battery_charge_attribute_id (str): InOrbit attribute definition for battery charge + charging_status_attribute_id (str): InOrbit attribute definition for charging status + actions (dict?): Optional. Information about each defined action. Currently supported actions: + dock (dict): + action_id (str): Action ID of the docking action + dock_name (str): 'dock_name' of the dock corresponding to the action + max_lin_speed (float): Maximum linear speed defined in the fleet config + logger (RcutilsLogger): Node logger + base_url (str): InOrbit API base url. Default: "https://api.inorbit.ai/robots/" + threshold_distance (float): Distance in meters from position to goal forwards or backwards. Threshold to goal before stop. Default is 0.15m + threshold_angle (float): Angle in radians from pose angle to goal pose each side. Threshold to goal before stop. Default is 0.13 (7.5°) + """ + + assert api_key, 'api_key is empty or None' + assert name, 'name is empty or None' + assert max_lin_speed > 0, 'max_linear_speed is not greater than zero' + assert logger, 'logger is None' + assert threshold_angle > 0 and threshold_distance > 0, "Proximity thresholds must be grater than zero" + + self.CONFIG_SCHEMA.validate(robot_config) + logger.info("RobotAPI configuration is valid") + + self.headers = {self.API_KEY_HEADER: api_key} + + self.name = name + self.robot_config = robot_config + self.max_lin_speed = max_lin_speed + self.logger = logger + self.dock_enabled = 'dock' in self.robot_config['actions'] + + self.requester = Requester( + base_url=base_url, + robot_id=self.robot_config['robot_id'], + headers=self.headers, + logger=self.logger) + + self.threshold_distance = threshold_distance + self.threshold_angle = threshold_angle + + # Test connectivity + self.connected = self.check_connection() + if self.connected: + self.logger.info("Successfully able to query API server") + else: + self.logger.warn("Unable to query API server") + + # Keep track of last requested navigation pose and process ID + self.last_requested_pose = None + self.last_requested_action = None + + def check_connection(self) -> bool: + ''' Return True if connection to the robot API server is successful''' + response = self.requester.robot_get_request("") + if not response: + self.logger.error("Exception calling InOrbit API") + return False + return response.status_code == 200 + + def position(self) -> list | None: + ''' Return [x, y, theta] expressed in the robot's coordinate frame or + None if any errors are encountered''' + response = self.requester.robot_get_request("/localization/pose") + if not response: + return None + res_json = response.json() + self.logger.info(f"position response: {res_json}") + if response.status_code != 200: + return None + return [res_json['x'], res_json['y'], res_json['theta']] + + def navigate(self, pose) -> bool: + ''' 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''' + self.logger.info(f"nav request: {pose}") + self.last_requested_pose = pose + response = self.requester.robot_post_request( + "/navigation/waypoints", json={'waypoints': [{'x': pose[0], 'y': pose[1], 'theta': pose[2]}]}) + if not response: + return False + res_json = response.json() + self.logger.info(f"nav response: {res_json}") + return response.status_code == 200 + + def start_docking(self, dock_name: str) -> bool: + ''' Request the robot to begin the docking process. It will compare the received dock name to the + dock name linked to the action set for this robot, and fail if they are different. + Return True if the robot has accepted the request, else False ''' + + if not self.dock_enabled: + return False + + assert dock_name == self.robot_config['actions']['dock'][ + 'dock_name'], "Tried to execute docking action into a non matching dock" + + action_id = self.robot_config['actions']['dock']['action_id'] + response = self.requester.robot_post_request( + "/actions", json={"actionId": action_id, "parameters": {}}) + + if not response: + return False + res_json = response.json() + self.logger.info(f"Start docking response: {res_json}") + + return response.status_code == 200 + + def is_robot_charging(self) -> bool | None: + ''' Returns whether the robot is charging ''' + + response = self.requester.robot_get_request( + f"/attributes/{self.robot_config['charging_status_attribute_id']}") + if not response: + return None + + res_json = response.json() + self.logger.info(f"Charging status response: {res_json}") + if not isinstance(res_json['value'], str): + return None + + return res_json['value'].lower() == 'charging' + + def stop(self) -> bool: + ''' Command the robot to stop. + Return True if robot has successfully stopped. Else False''' + response = self.requester.robot_post_request( + "/actions", json=self.STOP_COMMAND_PAYLOAD) + if not response: + return False + res_json = response.json() + self.logger.info(f"Stop response: {res_json}") + return response.status_code == 200 + + def navigation_remaining_duration(self): + ''' Return a rough estimate of the number of seconds remaining for the robot to reach + its destination + Estimated from the distance left at an speed lower than the maximum''' + try: + [curr_x, curr_y, _] = self.position() + [wp_x, wp_y, _] = self.last_requested_pose + distance_left = euc_distance(curr_x, curr_y, wp_x, wp_y) + except: + self.logger.error("Cannot estimate remaining duration") + return 0.0 + duration = distance_left / self.max_lin_speed + self.logger.info(f"Navigation remaining duration: {duration}s") + return duration + + def navigation_completed(self) -> list | None: + ''' Return True if the robot has successfully completed its previous + navigation request. Else False.''' + if self.last_requested_pose is None: + self.logger.warn( + "Navigation completed: No last pose, returning True") + return True + [ax, ay, atheta] = self.last_requested_pose + [bx, by, btheta] = self.position() + res = (euc_distance(ax, ay, bx, by) < self.threshold_distance and + math.fabs(angle_diff(atheta, btheta)) < self.threshold_angle) + self.logger.info(f"Navigation completed: {res}") + return res + + def battery_soc(self) -> int | None: + ''' 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.requester.robot_get_request( + f"/attributes/{self.robot_config['battery_charge_attribute_id']}") + if not response: + return None + res_json = response.json() + self.logger.info(f"battery SOC response: {res_json}") + if response.status_code != 200: + return None + return res_json['value'] diff --git a/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/RobotCommandHandle.py b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/RobotCommandHandle.py new file mode 100644 index 0000000..3b14d38 --- /dev/null +++ b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/RobotCommandHandle.py @@ -0,0 +1,472 @@ +# Copyright 2023 InOrbit, Inc. +# +# This file has been copied and adapted from +# https://github.com/open-rmf/fleet_adapter_template/blob/32f47a451ea67fa299a4fdb1a189d5db8df84b6b/fleet_adapter_template/fleet_adapter_template/RobotCommandHandle.py# +# The following is a summary of the introduced changes: +# - Adapted the initialization +# - Implemented _follow_path() , _dock() , get_position() +# - Changed the coordinate transformation handling +# +# The following is a copy of the original license note: +# +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from rclpy.duration import Duration + +import rmf_adapter as adpt + +import numpy as np + +import threading +import math +import enum +import time +from schema import Schema + +from datetime import timedelta + +from .RobotClientAPI import RobotAPI + + +# States for RobotCommandHandle's state machine used when guiding robot along +# a new path +class RobotState(enum.IntEnum): + IDLE = 0 + WAITING = 1 + MOVING = 2 + + +class RobotCommandHandle(adpt.RobotCommandHandle): + + MAP_SCHEMA = Schema({ + "x": float, + "y": float + }) + + def __init__(self, + robot: RobotAPI, + fleet_name, + node, + graph, + vehicle_traits, + map_name, + map_config, + start, + position, + charger_waypoint, + update_frequency, + adapter): + adpt.RobotCommandHandle.__init__(self) + self.fleet_name = fleet_name + self.node = node + self.graph = graph + self.vehicle_traits = vehicle_traits + self.map_name = map_name + self.MAP_SCHEMA.validate(map_config) + self.map_config = map_config + # Get the index of the charger waypoint + waypoint = self.graph.find_waypoint(charger_waypoint) + assert waypoint, f"Charger waypoint {charger_waypoint} \ + does not exist in the navigation graph" + self.charger_waypoint_index = waypoint.index + self.charger_is_set = False + self.update_frequency = update_frequency + self.update_handle = None # RobotUpdateHandle + self.battery_soc = 1.0 + self.robot = robot + # (x,y,theta) in RMF coordinates (meters, radians) + self.position = position + self.initialized = False + self.state = RobotState.IDLE + self.dock_name = "" + self.adapter = adapter + + self.requested_waypoints = [] # RMF Plan waypoints + self.remaining_waypoints = [] + self.path_finished_callback = None + self.next_arrival_estimator = None + self.path_index = 0 + self.docking_finished_callback = None + + # RMF location trackers + self.last_known_lane_index = None + self.last_known_waypoint_index = None + # if robot is waiting at a waypoint. This is a Graph::Waypoint index + self.on_waypoint = None + # if robot is traveling on a lane. This is a Graph::Lane index + self.on_lane = None + self.target_waypoint = None # this is a Plan::Waypoint + # The graph index of the waypoint the robot is currently docking into + self.dock_waypoint_index = None + + # Threading variables + self._lock = threading.Lock() + self._follow_path_thread = None + self._quit_path_event = threading.Event() + self._dock_thread = None + self._quit_dock_event = threading.Event() + + self.node.get_logger().info( + f"The robot is starting at: [{self.position[0]:.2f}, " + f"{self.position[1]:.2f}, {self.position[2]:.2f}]") + + # Update tracking variables + if start.lane is not None: # If the robot is on a lane + self.last_known_lane_index = start.lane + self.on_lane = start.lane + self.last_known_waypoint_index = start.waypoint + else: # Otherwise, the robot is on a waypoint + self.last_known_waypoint_index = start.waypoint + self.on_waypoint = start.waypoint + + self.state_update_timer = self.node.create_timer( + 1.0 / self.update_frequency, + self.update) + + self.initialized = True + + def sleep_for(self, seconds): + goal_time =\ + self.node.get_clock().now() + Duration(nanoseconds=1e9*seconds) + while (self.node.get_clock().now() <= goal_time): + time.sleep(0.001) + + def clear(self): + with self._lock: + self.requested_waypoints = [] + self.remaining_waypoints = [] + self.path_finished_callback = None + self.next_arrival_estimator = None + self.docking_finished_callback = None + self.state = RobotState.IDLE + + def stop(self): + # Stop the robot. Tracking variables should remain unchanged. + while True: + self.node.get_logger().info("Requesting robot to stop...") + if self.robot.stop(): + break + self.sleep_for(0.1) + if self._follow_path_thread is not None: + self._quit_path_event.set() + if self._follow_path_thread.is_alive(): + self._follow_path_thread.join() + self._follow_path_thread = None + self.clear() + + def transform_rmf_to_robot(self, x, y, theta): + # Convert pose from RMF to InOrbit's robot frame + # Returns [x, y, theta] + return x+self.map_config['x'], y-self.map_config['y'], theta + + def transform_robot_to_rmf(self, x, y, theta): + # Convert pose from InOrbit's robot frame to RMF + # Returns [x, y, theta] + return x-self.map_config['x'], y+self.map_config['y'], theta + + def follow_new_path( + self, + waypoints, + next_arrival_estimator, + path_finished_callback): + + self.stop() + self._quit_path_event.clear() + + self.node.get_logger().info("Received new path to follow...") + + self.remaining_waypoints = self.get_remaining_waypoints(waypoints) + assert next_arrival_estimator is not None + assert path_finished_callback is not None + self.next_arrival_estimator = next_arrival_estimator + self.path_finished_callback = path_finished_callback + + def _follow_path(): + target_pose = [] + while ( + self.remaining_waypoints or + self.state == RobotState.MOVING or + self.state == RobotState.WAITING): + # Check if we need to abort + if self._quit_path_event.is_set(): + self.node.get_logger().info("Aborting previously followed " + "path") + return + # State machine + if self.state == RobotState.IDLE: + # Assign the next waypoint + self.target_waypoint = self.remaining_waypoints[0][1] + self.path_index = self.remaining_waypoints[0][0] + # Move robot to next waypoint + target_pose = self.target_waypoint.position + self.node.get_logger().info(f"Coords are: {target_pose}") + x, y, theta = self.transform_rmf_to_robot(*target_pose) + response = self.robot.navigate([x, y, theta]) + + if response: + self.remaining_waypoints = self.remaining_waypoints[1:] + self.state = RobotState.MOVING + else: + self.node.get_logger().info( + f"Robot {self.robot.name} failed to navigate to " + f"[{x:.0f}, {y:.0f}, {theta:.0f}] coordinates. " + f"Retrying...") + self.sleep_for(0.1) + + elif self.state == RobotState.WAITING: + self.sleep_for(0.1) + time_now = self.adapter.now() + with self._lock: + if self.target_waypoint is not None: + waypoint_wait_time = self.target_waypoint.time + if (waypoint_wait_time < time_now): + self.state = RobotState.IDLE + else: + if self.path_index is not None: + self.node.get_logger().info( + f"Waiting for " + f"{(waypoint_wait_time - time_now).seconds}s", throttle_duration_sec=1) + self.next_arrival_estimator( + self.path_index, timedelta(seconds=0.0)) + + elif self.state == RobotState.MOVING: + self.sleep_for(0.1) + # Check if we have reached the target + with self._lock: + if (self.robot.navigation_completed()): + self.node.get_logger().info( + f"Robot [{self.robot.name}] has reached its target " + f"waypoint") + self.state = RobotState.WAITING + if (self.target_waypoint.graph_index is not None): + self.on_waypoint = \ + self.target_waypoint.graph_index + self.last_known_waypoint_index = \ + self.on_waypoint + else: + self.on_waypoint = None # still on a lane + else: + # Update the lane the robot is on + lane = self.get_current_lane() + if lane is not None: + self.on_waypoint = None + self.on_lane = lane + else: + # The robot may either be on the previous + # waypoint or the target one + if self.target_waypoint.graph_index is not \ + None and self.dist(self.position, target_pose) < 0.5: + self.on_waypoint = self.target_waypoint.graph_index + elif self.last_known_waypoint_index is not \ + None and self.dist( + self.position, self.graph.get_waypoint( + self.last_known_waypoint_index).location) < 0.5: + self.on_waypoint = self.last_known_waypoint_index + else: + self.on_lane = None # update_off_grid() + self.on_waypoint = None + duration = self.robot.navigation_remaining_duration() + if self.path_index is not None: + self.next_arrival_estimator( + self.path_index, timedelta(seconds=duration)) + self.path_finished_callback() + self.node.get_logger().info( + f"Robot {self.robot.name} has successfully navigated along " + f"requested path.") + + self._follow_path_thread = threading.Thread( + target=_follow_path) + self._follow_path_thread.start() + + def dock( + self, + dock_name, + docking_finished_callback): + ''' Starts the docking process ''' + self._quit_dock_event.clear() + if self._dock_thread is not None: + self._dock_thread.join() + + self.dock_name = dock_name + assert docking_finished_callback is not None + self.docking_finished_callback = docking_finished_callback + + # Get the waypoint that the robot is trying to dock into + dock_waypoint = self.graph.find_waypoint(self.dock_name) + assert (dock_waypoint) + self.dock_waypoint_index = dock_waypoint.index + + def _dock(): + # Request the robot to start the relevant process + self.node.get_logger().info( + f"Requesting robot {self.robot.name} to dock") + accepted = self.robot.start_docking(dock_name) + + if not accepted: + self.node.get_logger().warn( + f"Robot {self.robot.name} did not accept the docking request") + self.docking_finished_callback() + return + + with self._lock: + self.on_waypoint = None + self.on_lane = None + self.sleep_for(0.1) + while (not self.robot.is_robot_charging()): + # Check if we need to abort + if self._quit_dock_event.is_set(): + self.node.get_logger().info("Aborting docking") + return + self.node.get_logger().info("Robot is docking...", throttle_duration_sec=2) + self.sleep_for(0.5) + + with self._lock: + self.on_waypoint = self.dock_waypoint_index + self.dock_waypoint_index = None + self.docking_finished_callback() + self.node.get_logger().info("Docking completed") + + self._dock_thread = threading.Thread(target=_dock) + self._dock_thread.start() + + def get_position(self): + ''' This helper function returns the live position of the robot in the + RMF coordinate frame''' + position = self.robot.position() + if position is not None: + x, y, theta = self.transform_robot_to_rmf(*position) + # Wrap theta between [-pi, pi]. Else arrival estimate will + # assume robot has to do full rotations and delay the schedule + if theta > np.pi: + theta = theta - (2 * np.pi) + if theta < -np.pi: + theta = (2 * np.pi) + theta + return [x, y, theta] + else: + self.node.get_logger().error( + "Unable to retrieve position from robot.") + return self.position + + def get_battery_soc(self): + battery_soc = self.robot.battery_soc() + if battery_soc is not None: + return battery_soc + else: + self.node.get_logger().error( + "Unable to retrieve battery data from robot.") + return self.battery_soc + + def update(self): + self.position = self.get_position() + self.battery_soc = self.get_battery_soc() + if self.update_handle is not None: + self.update_state() + + def update_state(self): + self.update_handle.update_battery_soc(self.battery_soc) + if not self.charger_is_set: + if ("max_delay" in self.robot.robot_config.keys()): + max_delay = self.robot.robot_config["max_delay"] + self.node.get_logger().info( + f"Setting max delay to {max_delay}s") + self.update_handle.set_maximum_delay(max_delay) + if (self.charger_waypoint_index < self.graph.num_waypoints): + self.update_handle.set_charger_waypoint( + self.charger_waypoint_index) + else: + self.node.get_logger().warn( + "Invalid waypoint supplied for charger. " + "Using default nearest charger in the map") + self.charger_is_set = True + # Update position + with self._lock: + if (self.on_waypoint is not None): # if robot is on a waypoint + self.update_handle.update_current_waypoint( + self.on_waypoint, self.position[2]) + elif (self.on_lane is not None): # if robot is on a lane + # We only keep track of the forward lane of the robot. + # However, when calling this update it is recommended to also + # pass in the reverse lane so that the planner does not assume + # the robot can only head forwards. This would be helpful when + # the robot is still rotating on a waypoint. + forward_lane = self.graph.get_lane(self.on_lane) + entry_index = forward_lane.entry.waypoint_index + exit_index = forward_lane.exit.waypoint_index + reverse_lane = self.graph.lane_from(exit_index, entry_index) + lane_indices = [self.on_lane] + if reverse_lane is not None: # Unidirectional graph + lane_indices.append(reverse_lane.index) + self.update_handle.update_current_lanes( + self.position, lane_indices) + elif (self.dock_waypoint_index is not None): + self.update_handle.update_off_grid_position( + self.position, self.dock_waypoint_index) + # if robot is merging into a waypoint + elif (self.target_waypoint is not None and + self.target_waypoint.graph_index is not None): + self.update_handle.update_off_grid_position( + self.position, self.target_waypoint.graph_index) + else: # if robot is lost + self.update_handle.update_lost_position( + self.map_name, self.position) + + def get_current_lane(self): + def projection(current_position, + target_position, + lane_entry, + lane_exit): + px, py, _ = current_position + p = np.array([px, py]) + t = np.array(target_position) + entry = np.array(lane_entry) + exit = np.array(lane_exit) + return np.dot(p - t, exit - entry) + + if self.target_waypoint is None: + return None + approach_lanes = self.target_waypoint.approach_lanes + # Spin on the spot + if approach_lanes is None or len(approach_lanes) == 0: + return None + # Determine which lane the robot is currently on + for lane_index in approach_lanes: + lane = self.graph.get_lane(lane_index) + p0 = self.graph.get_waypoint(lane.entry.waypoint_index).location + p1 = self.graph.get_waypoint(lane.exit.waypoint_index).location + p = self.position + before_lane = projection(p, p0, p0, p1) < 0.0 + after_lane = projection(p, p1, p0, p1) >= 0.0 + if not before_lane and not after_lane: # The robot is on this lane + return lane_index + return None + + def dist(self, A, B): + ''' Euclidean distance between A(x,y) and B(x,y)''' + assert (len(A) > 1) + assert (len(B) > 1) + return math.sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) + + def get_remaining_waypoints(self, waypoints: list): + ''' + The function returns a list where each element is a tuple of the index + of the waypoint and the waypoint present in waypoints. This function + may be modified if waypoints in a path need to be filtered. + ''' + assert (len(waypoints) > 0) + remaining_waypoints = [] + + for i in range(len(waypoints)): + remaining_waypoints.append((i, waypoints[i])) + return remaining_waypoints diff --git a/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/__init__.py b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/fleet_adapter.py b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/fleet_adapter.py new file mode 100755 index 0000000..f4d0c0a --- /dev/null +++ b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/fleet_adapter.py @@ -0,0 +1,328 @@ +# Copyright 2023 InOrbit, Inc. +# +# This file has been copied and adapted from +# https://github.com/open-rmf/fleet_adapter_template/blob/32f47a451ea67fa299a4fdb1a189d5db8df84b6b/fleet_adapter_template/fleet_adapter_template/fleet_adapter.py# +# The following is a summary of the introduced changes: +# - Adapted the configuration file usage +# - Changed the parameter intake +# - Modified and moved the coordinate transformation to RobotCommandHandle +# - Adapter the initialization +# +# The following is a copy of the original license note: +# +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import argparse +import yaml +import time +import threading + +import rclpy +import rclpy.node +from rclpy.parameter import Parameter + +import rmf_adapter as adpt +import rmf_adapter.vehicletraits as traits +import rmf_adapter.battery as battery +import rmf_adapter.geometry as geometry +import rmf_adapter.graph as graph +import rmf_adapter.plan as plan + +from rmf_task_msgs.msg import TaskProfile, TaskType + +from functools import partial + +from .RobotCommandHandle import RobotCommandHandle +from .RobotClientAPI import RobotAPI + +# ------------------------------------------------------------------------------ +# Helper functions +# ------------------------------------------------------------------------------ + + +def initialize_fleet(config_yaml, api_key: str, nav_graph_path, node, use_sim_time, server_uri): + # Profile and traits + fleet_config = config_yaml['rmf_fleet'] + profile = traits.Profile(geometry.make_final_convex_circle( + fleet_config['profile']['footprint']), + geometry.make_final_convex_circle(fleet_config['profile']['vicinity'])) + vehicle_traits = traits.VehicleTraits( + linear=traits.Limits(*fleet_config['limits']['linear']), + angular=traits.Limits(*fleet_config['limits']['angular']), + profile=profile) + vehicle_traits.differential.reversible = fleet_config['reversible'] + + # Battery system + voltage = fleet_config['battery_system']['voltage'] + capacity = fleet_config['battery_system']['capacity'] + charging_current = fleet_config['battery_system']['charging_current'] + battery_sys = battery.BatterySystem.make( + voltage, capacity, charging_current) + + # Mechanical system + mass = fleet_config['mechanical_system']['mass'] + moment = fleet_config['mechanical_system']['moment_of_inertia'] + friction = fleet_config['mechanical_system']['friction_coefficient'] + mech_sys = battery.MechanicalSystem.make(mass, moment, friction) + + # Power systems + ambient_power_sys = battery.PowerSystem.make( + fleet_config['ambient_system']['power']) + tool_power_sys = battery.PowerSystem.make( + fleet_config['tool_system']['power']) + + # Power sinks + motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys) + ambient_sink = battery.SimpleDevicePowerSink( + battery_sys, ambient_power_sys) + tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys) + + nav_graph = graph.parse_graph(nav_graph_path, vehicle_traits) + + # Adapter + fleet_name = fleet_config['name'] + adapter = adpt.Adapter.make(f'{fleet_name}_fleet_adapter') + if use_sim_time: + adapter.node.use_sim_time() + assert adapter, ("Unable to initialize fleet adapter. Please ensure " + "RMF Schedule Node is running") + adapter.start() + time.sleep(1.0) + + fleet_handle = adapter.add_fleet( + fleet_name, vehicle_traits, nav_graph, server_uri) + + if not fleet_config['publish_fleet_state']: + fleet_handle.fleet_state_publish_period(None) + # Account for battery drain + drain_battery = fleet_config['account_for_battery_drain'] + recharge_threshold = fleet_config['recharge_threshold'] + recharge_soc = fleet_config['recharge_soc'] + finishing_request = fleet_config['task_capabilities']['finishing_request'] + node.get_logger().info(f"Finishing request: [{finishing_request}]") + # Set task planner params + ok = fleet_handle.set_task_planner_params( + battery_sys, + motion_sink, + ambient_sink, + tool_sink, + recharge_threshold, + recharge_soc, + drain_battery, + finishing_request) + assert ok, ("Unable to set task planner params") + + task_capabilities = [] + if fleet_config['task_capabilities']['loop']: + node.get_logger().info( + f"Fleet [{fleet_name}] is configured to perform Loop tasks") + task_capabilities.append(TaskType.TYPE_LOOP) + + # Callable for validating requests that this fleet can accommodate + def _task_request_check(task_capabilities, msg: TaskProfile): + if msg.description.task_type in task_capabilities: + return True + else: + return False + + fleet_handle.accept_task_requests( + partial(_task_request_check, task_capabilities)) + + def _updater_inserter(cmd_handle, update_handle): + """Insert a RobotUpdateHandle.""" + cmd_handle.update_handle = update_handle + + missing_robots = config_yaml['robots'] + + def _add_fleet_robots(): + while len(missing_robots) > 0: + time.sleep(0.2) + for robot_name in list(missing_robots.keys()): + + robots_config = config_yaml['robots'][robot_name] + robot_config = robots_config['robot_config'] + + robot = RobotAPI( + api_key=api_key, + name=robot_name, + robot_config=robot_config, + max_lin_speed=fleet_config['limits']['linear'][0], + logger=node.get_logger(), + threshold_distance=fleet_config['fleet_handle']['threshold_distance'], + threshold_angle=fleet_config['fleet_handle']['threshold_angle']) + + node.get_logger().info( + f"Connecting to robot: {robot.name} with id {robot.robot_config['robot_id']}") + position = robot.position() + node.get_logger().info(f"got pose: {position}") + if position is None: + continue + if len(position) > 2: + node.get_logger().info( + f"Initializing robot: {robot.name} with id {robot.robot_config['robot_id']}") + node.get_logger().info(f"Position: {position}") + rmf_config = robots_config['rmf_config'] + initial_waypoint = rmf_config['start']['waypoint'] + initial_orientation = rmf_config['start']['orientation'] + + starts = [] + time_now = adapter.now() + + if (initial_waypoint is not None) and\ + (initial_orientation is not None): + node.get_logger().info( + f"Using provided initial waypoint " + f"[{initial_waypoint}] " + f"and orientation [{initial_orientation:.2f}] to " + f"initialize starts for robot [{robot.name} with id {robot.robot_config['robot_id']}]") + # Get the waypoint index for initial_waypoint + initial_waypoint_index = nav_graph.find_waypoint( + initial_waypoint).index + starts = [plan.Start(time_now, + initial_waypoint_index, + initial_orientation)] + else: + node.get_logger().info( + f"Running compute_plan_starts for robot: " + f"{robot.name} with id {robot.robot_config['robot_id']}") + starts = plan.compute_plan_starts( + nav_graph, + rmf_config['start']['map_name'], + position, + time_now) + + if starts is None or len(starts) == 0: + node.get_logger().error( + f"Unable to determine StartSet for {robot.name} with id {robot.robot_config['robot_id']}") + continue + + robotCommandHandle = RobotCommandHandle( + robot=robot, + fleet_name=fleet_name, + node=node, + graph=nav_graph, + vehicle_traits=vehicle_traits, + map_name=rmf_config['start']['map_name'], + map_config=config_yaml['map'], + start=starts[0], + position=position, + charger_waypoint=rmf_config['charger']['waypoint'], + update_frequency=rmf_config.get( + 'robot_state_update_frequency', 1), + adapter=adapter) + + if robotCommandHandle.initialized: + # Add robot to fleet + fleet_handle.add_robot(robotCommandHandle, + robot.name, + profile, + [starts[0]], + partial(_updater_inserter, + robotCommandHandle)) + node.get_logger().info( + f"Successfully added new robot: {robot.name} with id {robot.robot_config['robot_id']}") + + else: + node.get_logger().error( + f"Failed to initialize robot: {robot.name} with id {robot.robot_config['robot_id']}") + + del missing_robots[robot.name] + + else: + pass + node.get_logger().debug( + f"{robot.name} with id {robot.robot_config['robot_id']} not found, trying again...") + return + + add_robots = threading.Thread(target=_add_fleet_robots, args=()) + add_robots.start() + return adapter + + +# ------------------------------------------------------------------------------ +# Main +# ------------------------------------------------------------------------------ +def main(argv=sys.argv): + # Init rclpy and adapter + rclpy.init(args=argv) + adpt.init_rclcpp() + args_without_ros = rclpy.utilities.remove_ros_args(argv) + + parser = argparse.ArgumentParser( + prog="fleet_adapter", + description="Configure and spin up the fleet adapter") + parser.add_argument("-c", "--config_file", type=str, required=True, + help="Path to the config.yaml file") + parser.add_argument("-k", "--api_key", type=str, required=True, + help='InOrbit API KEY, required') + parser.add_argument("-n", "--nav_graph", type=str, required=True, + help="Path to the nav_graph for this fleet adapter") + parser.add_argument("-s", "--server_uri", type=str, required=False, default="", + help="URI of the api server to transmit state and task information.") + parser.add_argument("--use_sim_time", action="store_true", + help='Use sim time, default: false') + args = parser.parse_args(args_without_ros[1:]) + print(f"Starting InOrbit fleet adapter...") + + config_path = args.config_file + nav_graph_path = args.nav_graph + + # Load config and nav graph yamls + with open(config_path, "r") as f: + config_yaml = yaml.safe_load(f) + + # ROS 2 node for the command handle + fleet_name = config_yaml['rmf_fleet']['name'] + node = rclpy.node.Node(f'inorbit_fleet_command_handle') + + node.get_logger().info( + f"Fleet [{fleet_name}] Node started") + + # Enable sim time for testing offline + if args.use_sim_time: + param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + node.set_parameters([param]) + + if args.server_uri == "": + server_uri = None + else: + server_uri = args.server_uri + + api_key = args.api_key + + initialize_fleet( + config_yaml, + api_key, + nav_graph_path, + node, + args.use_sim_time, + server_uri) + + # Create executor for the command handle node + rclpy_executor = rclpy.executors.SingleThreadedExecutor() + rclpy_executor.add_node(node) + + # Start the fleet adapter + rclpy_executor.spin() + + # Shutdown + node.destroy_node() + rclpy_executor.shutdown() + rclpy.shutdown() + + +if __name__ == '__main__': + main(sys.argv) diff --git a/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/utils.py b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/utils.py new file mode 100644 index 0000000..8bf78a9 --- /dev/null +++ b/rmf_inorbit_fleet_adapter/rmf_inorbit_fleet_adapter/utils.py @@ -0,0 +1,16 @@ +#!/usr/bin/env python3 +# Copyright 2023 InOrbit, Inc. + +"""Utility functions""" + +import math + + +def angle_diff(phi, theta): + """ Angle from theta to phi """ + return (phi - theta + math.pi) % (2 * math.pi) - math.pi + + +def euc_distance(x1, y1, x2, y2): + """ Euclidean distance between point 1 and 2 """ + return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) diff --git a/rmf_inorbit_fleet_adapter/setup.cfg b/rmf_inorbit_fleet_adapter/setup.cfg new file mode 100644 index 0000000..2232355 --- /dev/null +++ b/rmf_inorbit_fleet_adapter/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmf_inorbit_fleet_adapter +[install] +install_scripts=$base/lib/rmf_inorbit_fleet_adapter diff --git a/rmf_inorbit_fleet_adapter/setup.py b/rmf_inorbit_fleet_adapter/setup.py new file mode 100644 index 0000000..4310d01 --- /dev/null +++ b/rmf_inorbit_fleet_adapter/setup.py @@ -0,0 +1,31 @@ +from glob import glob +import os +from setuptools import setup + +package_name = 'rmf_inorbit_fleet_adapter' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), + glob('launch/*launch.[pxy][yma]*')), + (os.path.join('share', package_name), glob('config/*')) + ], + install_requires=['setuptools'], + zip_safe=True, + author="Tomás Badenes tomasbadenes@gmail.com", + maintainer='Julian Cerruti', + maintainer_email='julian@inorbit.ai', + description='Full Control Fleet Adapter for integrating InOrbit with Open-RMF', + license='3-Clause BSD License', + entry_points={ + 'console_scripts': [ + 'fleet_adapter=rmf_inorbit_fleet_adapter.fleet_adapter:main', + ], + }, +)