diff --git a/rmf_demos/launch/clinic.launch.xml b/rmf_demos/launch/clinic.launch.xml index 76b33190..d21f15d0 100644 --- a/rmf_demos/launch/clinic.launch.xml +++ b/rmf_demos/launch/clinic.launch.xml @@ -29,4 +29,18 @@ + + + + + + + + + + + + + + diff --git a/rmf_demos/launch/hotel.launch.xml b/rmf_demos/launch/hotel.launch.xml index d3b59431..097ce3b1 100644 --- a/rmf_demos/launch/hotel.launch.xml +++ b/rmf_demos/launch/hotel.launch.xml @@ -52,6 +52,20 @@ + + + + + + + + + + + + + + diff --git a/rmf_demos/launch/office.launch.xml b/rmf_demos/launch/office.launch.xml index 6b2ed460..95079bc8 100644 --- a/rmf_demos/launch/office.launch.xml +++ b/rmf_demos/launch/office.launch.xml @@ -20,4 +20,11 @@ + + + + + + + diff --git a/rmf_demos/launch/office_mock_traffic_light.launch.xml b/rmf_demos/launch/office_mock_traffic_light.launch.xml index d2f94b43..14e46f41 100644 --- a/rmf_demos/launch/office_mock_traffic_light.launch.xml +++ b/rmf_demos/launch/office_mock_traffic_light.launch.xml @@ -32,4 +32,11 @@ + + + + + + + diff --git a/rmf_demos_door_adapter/.gitignore b/rmf_demos_door_adapter/.gitignore new file mode 100644 index 00000000..bee8a64b --- /dev/null +++ b/rmf_demos_door_adapter/.gitignore @@ -0,0 +1 @@ +__pycache__ diff --git a/rmf_demos_door_adapter/README.md b/rmf_demos_door_adapter/README.md new file mode 100644 index 00000000..d6bb7359 --- /dev/null +++ b/rmf_demos_door_adapter/README.md @@ -0,0 +1,66 @@ +# rmf_demos_door_adapter + +Demo door adapter for integration with RMF + +## API Endpoints + +This door adapter integration relies on a door manager and a door adapter: +- The **door manager** comprises of specific endpoints that help relay commands to the simulated doors. It communicates with the doors over internal ROS 2 messages, while interfacing with the adapter via an API chosen by the user. For this demo door adapter implementation, we are using REST API with FastAPI framework. +- The **door adapter** receives commands from RMF and interfaces with the door manager to receive door state information, as well as query for available doors and send commands to open or close. + +To interact with endpoints, launch the demo and then visit http://127.0.0.1:5002/docs in your browser. + +### 1. Get Door Names +Get a list of the door names being managed by this adapter. This endpoint does not require a Request Body. + +Request URL: `http://127.0.0.1:5002/open-rmf/demo-door/door_names` +##### Response Body: +```json +{ + "data": { + "door_names": [ + "main_door_left", + "green_room_door", + "main_door_right" + ] + }, + "success": true, + "msg": "" +} +``` + + +### 2. Get Door State +Gets the state of the door with the specified name. This endpoint only requires a `door_name` query parameter. + +Request URL: `http://127.0.0.1:5002/open-rmf/demo-door/door_state?door_name=door` +##### Response Body: +```json +{ + "data": { + "current_mode": 0 + }, + "success": true, + "msg": "" +} +``` + +### 3. Send Door Request +The `door_request` endpoint allows the door adapter to send requests to a specified door. This endpoint requires a Request Body and a `door_name` query parameter. + +Request URL: `http://127.0.0.1:5002/open-rmf/demo-door/door_request?door_name=door` +##### Request Body: +```json +{ + "requested_mode": 0 +} +``` + +##### Response Body: +```json +{ + "data": {}, + "success": true, + "msg": "" +} +``` diff --git a/rmf_demos_door_adapter/launch/door_adapter.launch.xml b/rmf_demos_door_adapter/launch/door_adapter.launch.xml new file mode 100644 index 00000000..7824fb0f --- /dev/null +++ b/rmf_demos_door_adapter/launch/door_adapter.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rmf_demos_door_adapter/package.xml b/rmf_demos_door_adapter/package.xml new file mode 100644 index 00000000..08fe8ffc --- /dev/null +++ b/rmf_demos_door_adapter/package.xml @@ -0,0 +1,20 @@ + + + + rmf_demos_door_adapter + 2.0.2 + Example door adapter to be used with rmf_demos simulations + Luca Della Vedova + Apache 2.0 + + rmf_door_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/rmf_demos_door_adapter/resource/rmf_demos_door_adapter b/rmf_demos_door_adapter/resource/rmf_demos_door_adapter new file mode 100644 index 00000000..e69de29b diff --git a/rmf_demos_door_adapter/rmf_demos_door_adapter/DoorAPI.py b/rmf_demos_door_adapter/rmf_demos_door_adapter/DoorAPI.py new file mode 100644 index 00000000..44e60176 --- /dev/null +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/DoorAPI.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 __future__ import annotations + +import requests +from typing import Optional + +from rmf_door_msgs.msg import DoorMode +from rclpy.impl.rcutils_logger import RcutilsLogger + + +''' + The DoorAPI class is a wrapper for API calls to the door. Here users are + expected to fill up the implementations of functions which will be used by + the DoorAdapter. For example, if your door has a REST API, you will need to + make http request calls to the appropriate endpoints within these functions. +''' + + +class DoorAPI: + # The constructor accepts a safe loaded YAMLObject, which should contain all + # information that is required to run any of these API calls. + def __init__(self, address: str, port: int, logger: RcutilsLogger): + self.prefix = 'http://' + address + ':' + str(port) + self.logger = logger + self.timeout = 1.0 + + def door_mode(self, door_name: str) -> Optional[int]: + ''' Returns the DoorMode or None if the query failed''' + try: + response = requests.get(self.prefix + + f'/open-rmf/demo-door/door_state?door_name={door_name}', + timeout=self.timeout) + except Exception as err: + self.logger.info(f'{err}') + return None + if response.status_code != 200 or response.json()['success'] is False: + return None + # In this example the door uses the same API as RMF, if it didn't + # we would need to convert the result into a DoorMode here + door_mode = response.json()['data']['current_mode'] + return door_mode + + def _command_door(self, door_name: str, requested_mode: int) -> bool: + ''' Utility function to command doors. Returns True if the request + was sent out successfully, False otherwise''' + try: + data = {'requested_mode': requested_mode} + response = requests.post(self.prefix + + f'/open-rmf/demo-door/door_request?door_name={door_name}', + timeout=self.timeout, + json=data) + except Exception as err: + self.logger.info(f'{err}') + return None + if response.status_code != 200 or response.json()['success'] is False: + return False + return True + + def get_door_names(self) -> Optional[list]: + ''' Query the door manager for door names. Returns a list of door names + if the request was sent out successfully, None otherwise''' + try: + response = requests.get(self.prefix + + '/open-rmf/demo-door/door_names', + timeout=self.timeout) + except Exception as err: + self.logger.info(f'{err}') + return None + if response.status_code != 200 or response.json()['success'] is False: + return None + return response.json()['data']['door_names'] + + def open_door(self, door_name: str) -> bool: + ''' Command the door to open. Returns True if the request + was sent out successfully, False otherwise''' + return self._command_door(door_name, DoorMode.MODE_OPEN) + + def close_door(self, door_name: str) -> bool: + ''' Command the door to close. Returns True if the request + was sent out successfully, False otherwise''' + return self._command_door(door_name, DoorMode.MODE_CLOSED) diff --git a/rmf_demos_door_adapter/rmf_demos_door_adapter/__init__.py b/rmf_demos_door_adapter/rmf_demos_door_adapter/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py new file mode 100644 index 00000000..6da2fa2d --- /dev/null +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 +from typing import Optional + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default +from rmf_door_msgs.msg import DoorMode, DoorState, DoorRequest + +from .DoorAPI import DoorAPI + +''' + The DemoDoorAdapter is a node which provide updates to Open-RMF, as well + as handle incoming requests to control the integrated door, by calling the + implemented functions in DoorAPI. +''' + + +class DemoDoorAdapter(Node): + def __init__(self): + super().__init__('rmf_demos_door_adapter') + + address = self.declare_parameter('manager_address', 'localhost').value + port = self.declare_parameter('manager_port', 5002).value + self.door_api = DoorAPI(address, port, self.get_logger()) + self.doors = set() + + self.door_state_pub = self.create_publisher( + DoorState, + 'door_states', + qos_profile=qos_profile_system_default) + self.door_request_sub = self.create_subscription( + DoorRequest, + 'door_requests', + self.door_request_callback, + qos_profile=qos_profile_system_default) + self.pub_state_timer = self.create_timer(1.0, self.publish_states) + self.get_logger().info('Running DemoDoorAdapter') + + def _door_state(self, door_name) -> Optional[DoorState]: + new_state = DoorState() + new_state.door_time = self.get_clock().now().to_msg() + new_state.door_name = door_name + + door_mode = self.door_api.door_mode(door_name) + if door_mode is None: + self.get_logger().error('Unable to retrieve door mode') + return None + + new_state.current_mode.value = door_mode + return new_state + + def publish_states(self): + self.doors = self.door_api.get_door_names() + for door_name in self.doors: + door_state = self._door_state(door_name) + if door_state is None: + continue + self.door_state_pub.publish(door_state) + + def door_request_callback(self, msg): + if msg.door_name not in self.doors: + return + + if msg.requested_mode.value == DoorMode.MODE_OPEN: + self.door_api.open_door(msg.door_name) + + elif msg.requested_mode.value == DoorMode.MODE_CLOSED: + self.door_api.close_door(msg.door_name) + + +def main(argv=sys.argv): + rclpy.init() + node = DemoDoorAdapter() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/rmf_demos_door_adapter/rmf_demos_door_adapter/door_manager.py b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_manager.py new file mode 100644 index 00000000..7a63adc3 --- /dev/null +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_manager.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 threading + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default + +from rmf_door_msgs.msg import DoorState, DoorRequest + +from fastapi import FastAPI +import uvicorn +from typing import Optional +from pydantic import BaseModel + +app = FastAPI() + + +class Request(BaseModel): + requested_mode: int + + +class Response(BaseModel): + data: Optional[dict] = None + success: bool + msg: str + + +''' + The DoorManager class simulates a bridge between the door API, that + depends on the vendor and could be for example REST based, and the + simulated doors that operate using ROS2 messages. + Users can use this door to validate their door adapter in simulation +''' + + +class DoorManager(Node): + + def __init__(self, namespace='sim'): + super().__init__('door_manager') + + self.address = self.declare_parameter('manager_address', 'localhost').value + self.port = self.declare_parameter('manager_port', 5002).value + + self.door_states = {} + + # Setup publisher and subscriber + self.door_request_pub = self.create_publisher( + DoorRequest, + namespace + '/door_requests', + qos_profile=qos_profile_system_default) + + self.door_state_sub = self.create_subscription( + DoorState, + namespace + '/door_states', + self.door_state_cb, + qos_profile=qos_profile_system_default) + + @app.get('/open-rmf/demo-door/door_names', + response_model=Response) + async def door_names(): + response = { + 'data': {}, + 'success': False, + 'msg': '' + } + + response['data']['door_names'] = [name for name in self.door_states] + response['success'] = True + return response + + @app.get('/open-rmf/demo-door/door_state', + response_model=Response) + async def state(door_name: str): + response = { + 'data': {}, + 'success': False, + 'msg': '' + } + + if door_name not in self.door_states: + self.get_logger().warn(f'Door {door_name} not found') + return response + + state = self.door_states[door_name] + if state is None: + return response + + response['data']['current_mode'] = state.current_mode.value + response['success'] = True + return response + + @app.post('/open-rmf/demo-door/door_request', + response_model=Response) + async def request(door_name: str, mode: Request): + req = DoorRequest() + response = { + 'data': {}, + 'success': False, + 'msg': '' + } + + if door_name not in self.door_states: + self.get_logger().warn(f'Door {door_name} not being managed') + return response + + now = self.get_clock().now() + req.door_name = door_name + req.request_time = now.to_msg() + req.requested_mode.value = mode.requested_mode + req.requester_id = 'rmf_demos_door_manager' + + self.door_request_pub.publish(req) + response['success'] = True + return response + + def door_state_cb(self, msg): + self.door_states[msg.door_name] = msg + + +def main(argv=sys.argv): + rclpy.init() + node = DoorManager() + + spin_thread = threading.Thread(target=rclpy.spin, args=(node,)) + spin_thread.start() + + uvicorn.run(app, + host=node.address, + port=node.port, + log_level='warning') + + rclpy.shutdown() + + +if __name__ == '__main__': + main(sys.argv) diff --git a/rmf_demos_door_adapter/setup.cfg b/rmf_demos_door_adapter/setup.cfg new file mode 100644 index 00000000..f4d5b7c0 --- /dev/null +++ b/rmf_demos_door_adapter/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmf_demos_door_adapter +[install] +install_scripts=$base/lib/rmf_demos_door_adapter diff --git a/rmf_demos_door_adapter/setup.py b/rmf_demos_door_adapter/setup.py new file mode 100644 index 00000000..55a42ee1 --- /dev/null +++ b/rmf_demos_door_adapter/setup.py @@ -0,0 +1,31 @@ +import os +from glob import glob +from setuptools import setup + +package_name = 'rmf_demos_door_adapter' + +setup( + name=package_name, + version='2.0.2', + 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, 'launch'), + glob('launch/*.launch.xml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Luca Della Vedova', + maintainer_email='luca@openrobotics.org', + description='Demo door adapter to be used with rmf_demos simulations', + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'door_adapter = rmf_demos_door_adapter.door_adapter:main', + 'door_manager = rmf_demos_door_adapter.door_manager:main' + ], + }, +) diff --git a/rmf_demos_door_adapter/test/test_copyright.py b/rmf_demos_door_adapter/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/rmf_demos_door_adapter/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/rmf_demos_door_adapter/test/test_flake8.py b/rmf_demos_door_adapter/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/rmf_demos_door_adapter/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/rmf_demos_door_adapter/test/test_pep257.py b/rmf_demos_door_adapter/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/rmf_demos_door_adapter/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/rmf_demos_lift_adapter/.gitignore b/rmf_demos_lift_adapter/.gitignore new file mode 100644 index 00000000..bee8a64b --- /dev/null +++ b/rmf_demos_lift_adapter/.gitignore @@ -0,0 +1 @@ +__pycache__ diff --git a/rmf_demos_lift_adapter/README.md b/rmf_demos_lift_adapter/README.md new file mode 100644 index 00000000..b39dad49 --- /dev/null +++ b/rmf_demos_lift_adapter/README.md @@ -0,0 +1,72 @@ +# roscon_lift_adapter + +Lift adapter for the roscon workshop + +## API Endpoints + +This lift adapter integration relies on a lift manager and a lift adapter: +- The **lift manager** comprises of specific endpoints that help relay commands to the simulated lifts. It communicates with the lifts over internal ROS 2 messages, while interfacing with the adapter via an API chosen by the user. For this demo lift adapter implementation, we are using REST API with FastAPI framework. +- The **lift adapter** receives commands from RMF and interfaces with the lift manager to receive lift state information, as well as query for available lifts and send commands to go to different floors and open / close doors. + +To interact with endpoints, launch the demo and then visit http://127.0.0.1:5003/docs in your browser. + +### 1. Get Lift Names +Get a list of the lift names being managed by this adapter. This endpoint does not require a Request Body. + +Request URL: `http://127.0.0.1:5003/open-rmf/demo-lift/lift_names` +##### Response Body: +```json +{ + "data": { + "lift_names": [ + "lift" + ] + }, + "success": true, + "msg": "" +} +``` + + +### 2. Get Lift State +Gets the state of the lift with the specified name. This endpoint only requires a `lift_name` query parameter. + +Request URL: `http://127.0.0.1:5003/open-rmf/demo-lift/lift_state?lift_name=lift` +##### Response Body: +```json +{ + "data": { + "available_floors": [ + "L1", + "L2" + ], + "current_floor": "L1", + "destination_floor": "L1", + "door_state": 0, + "motion_state": 0 + }, + "success": true, + "msg": "" +} +``` + +### 3. Send Lift Request +The `lift_request` endpoint allows the lift adapter to send requests to a specified lift. This endpoint requires a Request Body and a `lift_name` query parameter. + +Request URL: `http://127.0.0.1:5003/open-rmf/demo-lift/lift_request?lift_name=lift` +##### Request Body: +```json +{ + "floor": "L2", + "door_state": 0 +} +``` + +##### Response Body: +```json +{ + "data": {}, + "success": true, + "msg": "" +} +``` diff --git a/rmf_demos_lift_adapter/launch/lift_adapter.launch.xml b/rmf_demos_lift_adapter/launch/lift_adapter.launch.xml new file mode 100644 index 00000000..77641221 --- /dev/null +++ b/rmf_demos_lift_adapter/launch/lift_adapter.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rmf_demos_lift_adapter/package.xml b/rmf_demos_lift_adapter/package.xml new file mode 100644 index 00000000..ca58b8a7 --- /dev/null +++ b/rmf_demos_lift_adapter/package.xml @@ -0,0 +1,20 @@ + + + + rmf_demos_lift_adapter + 2.0.2 + Example lift adapter to be used with rmf_demos simulations + Luca Della Vedova + Apache 2.0 + + rmf_lift_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/rmf_demos_lift_adapter/resource/rmf_demos_lift_adapter b/rmf_demos_lift_adapter/resource/rmf_demos_lift_adapter new file mode 100644 index 00000000..e69de29b diff --git a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py new file mode 100644 index 00000000..7cbc72ee --- /dev/null +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 __future__ import annotations + +import requests +from typing import Optional + +from rclpy.impl.rcutils_logger import RcutilsLogger +from rmf_lift_msgs.msg import LiftState + + +''' + The LiftAPI class is a wrapper for API calls to the lift. Here users are + expected to fill up the implementations of functions which will be used by + the LiftAdapter. For example, if your lift has a REST API, you will need to + make http request calls to the appropriate endpints within these functions. +''' + + +class LiftAPI: + # The constructor accepts a safe loaded YAMLObject, which should contain all + # information that is required to run any of these API calls. + def __init__(self, address: str, port: int, logger: RcutilsLogger): + self.prefix = 'http://' + address + ':' + str(port) + self.logger = logger + self.timeout = 1.0 + + def lift_state(self, lift_name: str) -> Optional[LiftState]: + ''' Returns the lift state or None if the query failed''' + try: + response = requests.get(self.prefix + + f'/open-rmf/demo-lift/lift_state?lift_name={lift_name}', + timeout=self.timeout) + except Exception as err: + self.logger.info(f'{err}') + return None + if response.status_code != 200 or response.json()['success'] is False: + return None + res_data = response.json()['data'] + lift_state = LiftState() + lift_state.lift_name = lift_name + lift_state.available_floors = res_data['available_floors'] + lift_state.door_state = res_data['door_state'] + lift_state.motion_state = res_data['motion_state'] + lift_state.current_floor = res_data['current_floor'] + lift_state.destination_floor = res_data['destination_floor'] + return lift_state + + def get_lift_names(self) -> Optional[list]: + ''' Returns a list of lift names or None if the query failed''' + try: + response = requests.get(self.prefix + + '/open-rmf/demo-lift/lift_names', + timeout=self.timeout) + except Exception as err: + self.logger.info(f'{err}') + return None + if response.status_code != 200 or response.json()['success'] is False: + return None + return response.json()['data']['lift_names'] + + def command_lift(self, lift_name: str, floor: str, door_state: int) -> bool: + ''' Sends the lift cabin to a specific floor and opens all available + doors for that floor. Returns True if the request was sent out + successfully, False otherwise''' + data = {'floor': floor, 'door_state': door_state} + try: + response = requests.post(self.prefix + + f'/open-rmf/demo-lift/lift_request?lift_name={lift_name}', + timeout=self.timeout, + json=data) + except Exception as err: + self.logger.info(f'{err}') + return None + if response.status_code != 200 or response.json()['success'] is False: + return False + return True diff --git a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/__init__.py b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py new file mode 100644 index 00000000..7633539e --- /dev/null +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 +from typing import Optional + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default +from rmf_lift_msgs.msg import LiftState, LiftRequest + +from .LiftAPI import LiftAPI + +''' + The DemoLiftAdapter is a node which provide updates to Open-RMF, as well + as handle incoming requests to control the integrated lift, by calling the + implemented functions in LiftAPI. +''' + + +class DemoLiftAdapter(Node): + def __init__(self): + super().__init__('rmf_demos_lift_adapter') + + self.lift_states = {} + self.lift_requests = {} + + address = self.declare_parameter('manager_address', 'localhost').value + port = self.declare_parameter('manager_port', 5003).value + + self.lift_api = LiftAPI(address, port, self.get_logger()) + + self.lift_state_pub = self.create_publisher( + LiftState, + 'lift_states', + qos_profile=qos_profile_system_default) + self.lift_request_sub = self.create_subscription( + LiftRequest, + 'lift_requests', + self.lift_request_callback, + qos_profile=qos_profile_system_default) + self.update_timer = self.create_timer(0.5, self.update_callback) + self.pub_state_timer = self.create_timer(1.0, self.publish_states) + self.get_logger().info('Running DemoLiftAdapter') + + def update_callback(self): + new_states = {} + lift_names = self.lift_api.get_lift_names() + for lift_name in lift_names: + if lift_name not in self.lift_requests: + self.lift_requests[lift_name] = None + + new_state = self._lift_state(lift_name) + new_states[lift_name] = new_state + if new_state is None: + self.get_logger().error( + f'Unable to get new state from lift {lift_name}') + continue + + lift_request = self.lift_requests[lift_name] + # No request to consider + if lift_request is None: + continue + + # If all is done, set request to None + if lift_request.destination_floor == \ + new_state.current_floor and \ + new_state.door_state == LiftState.DOOR_OPEN: + lift_request = None + self.lift_states = new_states + + def _lift_state(self, lift_name) -> Optional[LiftState]: + new_state = LiftState() + new_state.lift_time = self.get_clock().now().to_msg() + new_state.lift_name = lift_name + + lift_state = self.lift_api.lift_state(lift_name) + if lift_state is None: + self.get_logger().error('Unable to retrieve lift state') + return None + + new_state.available_floors = lift_state.available_floors + new_state.current_floor = lift_state.current_floor + new_state.destination_floor = lift_state.destination_floor + new_state.door_state = lift_state.door_state + new_state.motion_state = lift_state.motion_state + + new_state.available_modes = [LiftState.MODE_HUMAN, LiftState.MODE_AGV] + new_state.current_mode = LiftState.MODE_AGV + + lift_request = self.lift_requests[lift_name] + if lift_request is not None: + if lift_request.request_type == \ + LiftRequest.REQUEST_END_SESSION: + new_state.session_id = '' + else: + new_state.session_id = lift_request.session_id + return new_state + + def publish_states(self): + for lift_name, lift_state in self.lift_states.items(): + if lift_state is None: + self.get_logger().info('No lift state received for lift ' + f'{lift_name}') + continue + self.lift_state_pub.publish(lift_state) + + def lift_request_callback(self, msg): + if msg.lift_name not in self.lift_states: + return + + lift_state = self.lift_states[msg.lift_name] + if lift_state is not None and \ + msg.destination_floor not in lift_state.available_floors: + self.get_logger().info( + 'Floor {} not available.'.format(msg.destination_floor)) + return + + if not self.lift_api.command_lift(msg.lift_name, msg.destination_floor, msg.door_state): + self.get_logger().error( + f'Failed to send lift to {msg.destination_floor}.') + return + + self.lift_requests[msg.lift_name] = msg + + +def main(argv=sys.argv): + rclpy.init() + node = DemoLiftAdapter() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py new file mode 100644 index 00000000..a3a69c06 --- /dev/null +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 + +# Copyright 2022 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 threading + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default + +from rmf_lift_msgs.msg import LiftState, LiftRequest + +from fastapi import FastAPI +import uvicorn +from typing import Optional +from pydantic import BaseModel + +app = FastAPI() + + +class Request(BaseModel): + floor: str + door_state: int + + +class Response(BaseModel): + data: Optional[dict] = None + success: bool + msg: str + + +''' + The LiftManager class simulates a bridge between the lift API, that + depends on the vendor and could be for example REST based, and the + simulated lifts that operate using ROS2 messages. + Users can use this lift to validate their lift adapter in simulation +''' + + +class LiftManager(Node): + + def __init__(self, namespace='sim'): + super().__init__('lift_manager') + + self.address = self.declare_parameter('manager_address', 'localhost').value + self.port = self.declare_parameter('manager_port', 5003).value + + self.lift_states = {} + + # Setup publisher and subscriber + self.lift_request_pub = self.create_publisher( + LiftRequest, + namespace + '/lift_requests', + qos_profile=qos_profile_system_default) + + self.lift_state_sub = self.create_subscription( + LiftState, + namespace + '/lift_states', + self.lift_state_cb, + qos_profile=qos_profile_system_default) + + @app.get('/open-rmf/demo-lift/lift_state', + response_model=Response) + async def state(lift_name: str): + response = { + 'data': {}, + 'success': False, + 'msg': '' + } + + if lift_name not in self.lift_states: + self.get_logger().warn(f'Lift {lift_name} not being managed') + return response + + state = self.lift_states[lift_name] + + response['data']['available_floors'] = state.available_floors + response['data']['current_floor'] = state.current_floor + response['data']['destination_floor'] = state.destination_floor + response['data']['door_state'] = state.door_state + response['data']['motion_state'] = state.motion_state + response['success'] = True + return response + + @app.get('/open-rmf/demo-lift/lift_names', + response_model=Response) + async def lift_names(): + response = { + 'data': {}, + 'success': False, + 'msg': '' + } + + response['data']['lift_names'] = [name for name in self.lift_states] + response['success'] = True + return response + + @app.post('/open-rmf/demo-lift/lift_request', + response_model=Response) + async def request(lift_name: str, floor: Request): + req = LiftRequest() + response = { + 'data': {}, + 'success': False, + 'msg': '' + } + + if lift_name not in self.lift_states: + self.get_logger().warn(f'Lift {lift_name} not being managed') + return response + + now = self.get_clock().now() + req.lift_name = lift_name + req.request_time = now.to_msg() + req.request_type = req.REQUEST_AGV_MODE + req.door_state = floor.door_state + req.destination_floor = floor.floor + req.session_id = 'rmf_demos_lift_adapter' + + self.lift_request_pub.publish(req) + response['success'] = True + return response + + def lift_state_cb(self, msg): + self.lift_states[msg.lift_name] = msg + + +def main(argv=sys.argv): + rclpy.init() + node = LiftManager() + + spin_thread = threading.Thread(target=rclpy.spin, args=(node,)) + spin_thread.start() + + uvicorn.run(app, + host=node.address, + port=node.port, + log_level='warning') + + rclpy.shutdown() + + +if __name__ == '__main__': + main(sys.argv) diff --git a/rmf_demos_lift_adapter/setup.cfg b/rmf_demos_lift_adapter/setup.cfg new file mode 100644 index 00000000..5e38736a --- /dev/null +++ b/rmf_demos_lift_adapter/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmf_demos_lift_adapter +[install] +install_scripts=$base/lib/rmf_demos_lift_adapter diff --git a/rmf_demos_lift_adapter/setup.py b/rmf_demos_lift_adapter/setup.py new file mode 100644 index 00000000..7a696fe5 --- /dev/null +++ b/rmf_demos_lift_adapter/setup.py @@ -0,0 +1,31 @@ +import os +from glob import glob +from setuptools import setup + +package_name = 'rmf_demos_lift_adapter' + +setup( + name=package_name, + version='2.0.2', + 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, 'launch'), + glob('launch/*.launch.xml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Luca Della Vedova', + maintainer_email='luca@openrobotics.org', + description='Demo lift adapter to be used with rmf_demos simulations', + license='Apache License, Version 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'lift_adapter = rmf_demos_lift_adapter.lift_adapter:main', + 'lift_manager = rmf_demos_lift_adapter.lift_manager:main' + ], + }, +) diff --git a/rmf_demos_lift_adapter/test/test_copyright.py b/rmf_demos_lift_adapter/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/rmf_demos_lift_adapter/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/rmf_demos_lift_adapter/test/test_flake8.py b/rmf_demos_lift_adapter/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/rmf_demos_lift_adapter/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/rmf_demos_lift_adapter/test/test_pep257.py b/rmf_demos_lift_adapter/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/rmf_demos_lift_adapter/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'