From a9a5157fa20129773d095c1d093c0c7510088447 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 22 Nov 2022 15:26:08 +0800 Subject: [PATCH 1/7] Add demo door and lift adapters Signed-off-by: Luca Della Vedova --- rmf_demos_door_adapter/.gitignore | 1 + .../launch/door_adapter.launch.xml | 27 +++ rmf_demos_door_adapter/package.xml | 20 +++ .../resource/rmf_demos_door_adapter | 0 .../rmf_demos_door_adapter/DoorAPI.py | 96 +++++++++++ .../rmf_demos_door_adapter/__init__.py | 0 .../rmf_demos_door_adapter/door_adapter.py | 110 ++++++++++++ .../rmf_demos_door_adapter/door_manager.py | 153 +++++++++++++++++ rmf_demos_door_adapter/setup.cfg | 4 + rmf_demos_door_adapter/setup.py | 31 ++++ rmf_demos_door_adapter/test/test_copyright.py | 23 +++ rmf_demos_door_adapter/test/test_flake8.py | 25 +++ rmf_demos_door_adapter/test/test_pep257.py | 23 +++ rmf_demos_lift_adapter/.gitignore | 1 + .../launch/lift_adapter.launch.xml | 27 +++ rmf_demos_lift_adapter/package.xml | 20 +++ .../resource/rmf_demos_lift_adapter | 0 .../rmf_demos_lift_adapter/LiftAPI.py | 84 +++++++++ .../rmf_demos_lift_adapter/__init__.py | 0 .../rmf_demos_lift_adapter/lift_adapter.py | 159 +++++++++++++++++ .../rmf_demos_lift_adapter/lift_manager.py | 161 ++++++++++++++++++ rmf_demos_lift_adapter/setup.cfg | 4 + rmf_demos_lift_adapter/setup.py | 31 ++++ rmf_demos_lift_adapter/test/test_copyright.py | 23 +++ rmf_demos_lift_adapter/test/test_flake8.py | 25 +++ rmf_demos_lift_adapter/test/test_pep257.py | 23 +++ 26 files changed, 1071 insertions(+) create mode 100644 rmf_demos_door_adapter/.gitignore create mode 100644 rmf_demos_door_adapter/launch/door_adapter.launch.xml create mode 100644 rmf_demos_door_adapter/package.xml create mode 100644 rmf_demos_door_adapter/resource/rmf_demos_door_adapter create mode 100644 rmf_demos_door_adapter/rmf_demos_door_adapter/DoorAPI.py create mode 100644 rmf_demos_door_adapter/rmf_demos_door_adapter/__init__.py create mode 100644 rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py create mode 100644 rmf_demos_door_adapter/rmf_demos_door_adapter/door_manager.py create mode 100644 rmf_demos_door_adapter/setup.cfg create mode 100644 rmf_demos_door_adapter/setup.py create mode 100644 rmf_demos_door_adapter/test/test_copyright.py create mode 100644 rmf_demos_door_adapter/test/test_flake8.py create mode 100644 rmf_demos_door_adapter/test/test_pep257.py create mode 100644 rmf_demos_lift_adapter/.gitignore create mode 100644 rmf_demos_lift_adapter/launch/lift_adapter.launch.xml create mode 100644 rmf_demos_lift_adapter/package.xml create mode 100644 rmf_demos_lift_adapter/resource/rmf_demos_lift_adapter create mode 100644 rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py create mode 100644 rmf_demos_lift_adapter/rmf_demos_lift_adapter/__init__.py create mode 100644 rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py create mode 100644 rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py create mode 100644 rmf_demos_lift_adapter/setup.cfg create mode 100644 rmf_demos_lift_adapter/setup.py create mode 100644 rmf_demos_lift_adapter/test/test_copyright.py create mode 100644 rmf_demos_lift_adapter/test/test_flake8.py create mode 100644 rmf_demos_lift_adapter/test/test_pep257.py 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/launch/door_adapter.launch.xml b/rmf_demos_door_adapter/launch/door_adapter.launch.xml new file mode 100644 index 00000000..1d197a12 --- /dev/null +++ b/rmf_demos_door_adapter/launch/door_adapter.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + 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..c77d7dd3 --- /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 yaml import YAMLObject +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, config: YAMLObject, logger: RcutilsLogger): + self.config = config + self.prefix = 'http://' + config['door_manager']['ip'] +\ + ':' + str(config['door_manager']['port']) + self.logger = logger + self.timeout = 1.0 + + def door_mode(self, door_name) -> 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, 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 open_door(self, door_name): + """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): + """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..5600b371 --- /dev/null +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py @@ -0,0 +1,110 @@ +#!/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 yaml +import argparse +from typing import Optional +from yaml import YAMLObject + +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, args, config: YAMLObject): + super().__init__('rmf_demos_door_adapter') + + self.door_config = config + self.door_api = DoorAPI(self.door_config, self.get_logger()) + self.doors = set(config['doors']) + + 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): + for door_name in self.doors: + door_state = self._door_state(door_name) + if door_state is None: + self.get_logger().info('No door state received for door ' + f'{door_name}') + 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) + self.get_logger().info(f'Requested to open door {msg.door_name}') + + elif msg.requested_mode.value == DoorMode.MODE_CLOSED: + self.door_api.close_door(msg.door_name) + self.get_logger().info(f'Requested to close door {msg.door_name}') + + +def main(argv=sys.argv): + args_without_ros = rclpy.utilities.remove_ros_args(argv) + parser = argparse.ArgumentParser( + prog='rmf_demos_door_adapter', + description='RMF Demos door adapter') + parser.add_argument('-c', '--config', required=True, type=str) + args = parser.parse_args(args_without_ros[1:]) + + with open(args.config, 'r') as f: + config = yaml.safe_load(f) + + rclpy.init() + node = DemoDoorAdapter(args, config) + 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..2589b2fe --- /dev/null +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_manager.py @@ -0,0 +1,153 @@ +#!/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 argparse +import yaml + +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, doors, namespace='sim'): + super().__init__('door_manager') + + self.door_states = {} + for door in doors: + self.door_states[door] = None + + # 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_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('Door not being managed') + 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): + if msg.door_name not in self.door_states: + return + self.door_states[msg.door_name] = msg + + +def main(argv=sys.argv): + args_without_ros = rclpy.utilities.remove_ros_args(argv) + parser = argparse.ArgumentParser( + prog='door_manager', + description='Demo door manager') + parser.add_argument('-c', '--config', required=True, type=str) + args = parser.parse_args(args_without_ros[1:]) + + with open(args.config, 'r') as f: + config = yaml.safe_load(f) + + rclpy.init() + node = DoorManager(config['doors']) + + spin_thread = threading.Thread(target=rclpy.spin, args=(node,)) + spin_thread.start() + + uvicorn.run(app, + host=config['door_manager']['ip'], + port=config['door_manager']['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/launch/lift_adapter.launch.xml b/rmf_demos_lift_adapter/launch/lift_adapter.launch.xml new file mode 100644 index 00000000..8ca857a9 --- /dev/null +++ b/rmf_demos_lift_adapter/launch/lift_adapter.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + 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..2d9794e1 --- /dev/null +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py @@ -0,0 +1,84 @@ +#!/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 yaml import YAMLObject +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 endpoints + 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, config: YAMLObject, logger: RcutilsLogger): + self.config = config + self.prefix = 'http://' + config['lift_manager']['ip'] +\ + ':' + str(config['lift_manager']['port']) + self.logger = logger + self.timeout = 1.0 + + def lift_state(self, lift_name) -> 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 command_lift(self, lift_name, 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..3f336844 --- /dev/null +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py @@ -0,0 +1,159 @@ +#!/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 yaml +import argparse +from typing import Optional +from yaml import YAMLObject + +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, args, config: YAMLObject): + super().__init__('rmf_demos_lift_adapter') + + self.lift_states = {} + self.lift_requests = {} + self.lift_config = config + self.lift_api = LiftAPI(self.lift_config, self.get_logger()) + for lift in config['lifts']: + self.lift_requests[lift] = None + self.lift_states[lift] = self._lift_state(lift) + + 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 = {} + for lift_name, lift_state in self.lift_states.items(): + 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.get_logger().info(f'Requested lift {msg.lift_name} ' + f'to {msg.destination_floor}.') + self.lift_requests[msg.lift_name] = msg + + +def main(argv=sys.argv): + args_without_ros = rclpy.utilities.remove_ros_args(argv) + parser = argparse.ArgumentParser( + prog='rmf_demos_lift_adapter', + description='RMF Demos lift adapter') + parser.add_argument('-c', '--config', required=True, type=str) + args = parser.parse_args(args_without_ros[1:]) + + with open(args.config, 'r') as f: + config = yaml.safe_load(f) + + rclpy.init() + node = DemoLiftAdapter(args, config) + 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..9bcce4fb --- /dev/null +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py @@ -0,0 +1,161 @@ +#!/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 argparse +import yaml + +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, lifts, namespace='sim'): + super().__init__('lift_manager') + + self.lift_states = {} + for lift in lifts: + self.lift_states[lift] = None + + # 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('Lift not being managed') + return response + + state = self.lift_states[lift_name] + if state is None: + self.get_logger().warn('Lift state not received') + return response + + 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.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('Lift 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 = req.lift_name + '-' + str(now) + + self.lift_request_pub.publish(req) + response['success'] = True + return response + + def lift_state_cb(self, msg): + if msg.lift_name not in self.lift_states: + return + self.lift_states[msg.lift_name] = msg + + +def main(argv=sys.argv): + args_without_ros = rclpy.utilities.remove_ros_args(argv) + parser = argparse.ArgumentParser( + prog='lift_manager', + description='Demo lift manager') + parser.add_argument('-c', '--config', required=True, type=str) + args = parser.parse_args(args_without_ros[1:]) + + with open(args.config, 'r') as f: + config = yaml.safe_load(f) + + rclpy.init() + node = LiftManager(config['lifts']) + + spin_thread = threading.Thread(target=rclpy.spin, args=(node,)) + spin_thread.start() + + uvicorn.run(app, + host=config['lift_manager']['ip'], + port=config['lift_manager']['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' From a425c572c277ab38cf856d02c0657604aa6507ea Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 7 Dec 2022 10:30:08 +0800 Subject: [PATCH 2/7] WIP add launch for infrastructure adapters Signed-off-by: Luca Della Vedova --- rmf_demos/launch/office.launch.xml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/rmf_demos/launch/office.launch.xml b/rmf_demos/launch/office.launch.xml index 6b2ed460..f3ccb66d 100644 --- a/rmf_demos/launch/office.launch.xml +++ b/rmf_demos/launch/office.launch.xml @@ -20,4 +20,22 @@ + + + + + + + + + + + From 80c0269e8249f675aca57c0834bbc84943bc3f51 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 8 Dec 2022 11:39:18 +0800 Subject: [PATCH 3/7] Make doors discoverable Signed-off-by: Luca Della Vedova --- rmf_demos/launch/office.launch.xml | 2 - .../launch/door_adapter.launch.xml | 19 +++--- .../rmf_demos_door_adapter/DoorAPI.py | 64 +++++++++---------- .../rmf_demos_door_adapter/door_adapter.py | 29 +++------ .../rmf_demos_door_adapter/door_manager.py | 43 ++++++------- 5 files changed, 70 insertions(+), 87 deletions(-) diff --git a/rmf_demos/launch/office.launch.xml b/rmf_demos/launch/office.launch.xml index f3ccb66d..00a167ba 100644 --- a/rmf_demos/launch/office.launch.xml +++ b/rmf_demos/launch/office.launch.xml @@ -25,7 +25,6 @@ - --> @@ -34,7 +33,6 @@ - diff --git a/rmf_demos_door_adapter/launch/door_adapter.launch.xml b/rmf_demos_door_adapter/launch/door_adapter.launch.xml index 1d197a12..7824fb0f 100644 --- a/rmf_demos_door_adapter/launch/door_adapter.launch.xml +++ b/rmf_demos_door_adapter/launch/door_adapter.launch.xml @@ -3,25 +3,22 @@ - + + - - + + + - - + + + diff --git a/rmf_demos_door_adapter/rmf_demos_door_adapter/DoorAPI.py b/rmf_demos_door_adapter/rmf_demos_door_adapter/DoorAPI.py index c77d7dd3..44e60176 100644 --- a/rmf_demos_door_adapter/rmf_demos_door_adapter/DoorAPI.py +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/DoorAPI.py @@ -17,35 +17,30 @@ from __future__ import annotations import requests -from yaml import YAMLObject 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. -""" +''' + 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, config: YAMLObject, logger: RcutilsLogger): - self.config = config - self.prefix = 'http://' + config['door_manager']['ip'] +\ - ':' + str(config['door_manager']['port']) + 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) -> Optional[int]: - """Returns the DoorMode or None if the query failed.""" + 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}', @@ -60,12 +55,9 @@ def door_mode(self, door_name) -> Optional[int]: door_mode = response.json()['data']['current_mode'] return door_mode - def _command_door(self, door_name, requested_mode: int) -> bool: - """Utility function to command doors. - - Returns True if the request was sent out successfully, False - otherwise - """ + 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 + @@ -79,18 +71,26 @@ def _command_door(self, door_name, requested_mode: int) -> bool: return False return True - def open_door(self, door_name): - """Command the door to open. + 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'] - Returns True if the request was sent out successfully, False - otherwise - """ + 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): - """Command the door to close. - - Returns True if the request was sent out successfully, False - otherwise - """ + 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/door_adapter.py b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py index 5600b371..3f636dbb 100644 --- a/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py @@ -15,10 +15,7 @@ # limitations under the License. import sys -import yaml -import argparse from typing import Optional -from yaml import YAMLObject import rclpy from rclpy.node import Node @@ -27,20 +24,21 @@ 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, args, config: YAMLObject): + def __init__(self): super().__init__('rmf_demos_door_adapter') - self.door_config = config - self.door_api = DoorAPI(self.door_config, self.get_logger()) - self.doors = set(config['doors']) + 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, @@ -68,6 +66,7 @@ def _door_state(self, door_name) -> Optional[DoorState]: 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: @@ -90,18 +89,8 @@ def door_request_callback(self, msg): def main(argv=sys.argv): - args_without_ros = rclpy.utilities.remove_ros_args(argv) - parser = argparse.ArgumentParser( - prog='rmf_demos_door_adapter', - description='RMF Demos door adapter') - parser.add_argument('-c', '--config', required=True, type=str) - args = parser.parse_args(args_without_ros[1:]) - - with open(args.config, 'r') as f: - config = yaml.safe_load(f) - rclpy.init() - node = DemoDoorAdapter(args, config) + node = DemoDoorAdapter() rclpy.spin(node) rclpy.shutdown() 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 index 2589b2fe..7a63adc3 100644 --- a/rmf_demos_door_adapter/rmf_demos_door_adapter/door_manager.py +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_manager.py @@ -17,9 +17,6 @@ import sys import threading -import argparse -import yaml - import rclpy from rclpy.node import Node from rclpy.qos import qos_profile_system_default @@ -54,12 +51,13 @@ class Response(BaseModel): class DoorManager(Node): - def __init__(self, doors, namespace='sim'): + 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 = {} - for door in doors: - self.door_states[door] = None # Setup publisher and subscriber self.door_request_pub = self.create_publisher( @@ -73,6 +71,19 @@ def __init__(self, doors, namespace='sim'): 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): @@ -83,7 +94,7 @@ async def state(door_name: str): } if door_name not in self.door_states: - self.get_logger().warn('Door not being managed') + self.get_logger().warn(f'Door {door_name} not found') return response state = self.door_states[door_name] @@ -119,31 +130,19 @@ async def request(door_name: str, mode: Request): return response def door_state_cb(self, msg): - if msg.door_name not in self.door_states: - return self.door_states[msg.door_name] = msg def main(argv=sys.argv): - args_without_ros = rclpy.utilities.remove_ros_args(argv) - parser = argparse.ArgumentParser( - prog='door_manager', - description='Demo door manager') - parser.add_argument('-c', '--config', required=True, type=str) - args = parser.parse_args(args_without_ros[1:]) - - with open(args.config, 'r') as f: - config = yaml.safe_load(f) - rclpy.init() - node = DoorManager(config['doors']) + node = DoorManager() spin_thread = threading.Thread(target=rclpy.spin, args=(node,)) spin_thread.start() uvicorn.run(app, - host=config['door_manager']['ip'], - port=config['door_manager']['port'], + host=node.address, + port=node.port, log_level='warning') rclpy.shutdown() From 268ed6a9c5d8ee0d8b9bc82b702d9c45f5f54ac6 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 8 Dec 2022 11:40:41 +0800 Subject: [PATCH 4/7] Add README Signed-off-by: Luca Della Vedova --- rmf_demos_door_adapter/README.md | 66 ++++++++++++++++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100644 rmf_demos_door_adapter/README.md 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": "" +} +``` From c5ab8260f01e6c31b13509bcb8926c06a7a3af33 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 8 Dec 2022 11:46:26 +0800 Subject: [PATCH 5/7] Add adapters to launch files Signed-off-by: Luca Della Vedova --- rmf_demos/launch/clinic.launch.xml | 14 +++++++++++++ rmf_demos/launch/hotel.launch.xml | 14 +++++++++++++ rmf_demos/launch/office.launch.xml | 21 ++++++------------- .../office_mock_traffic_light.launch.xml | 7 +++++++ 4 files changed, 41 insertions(+), 15 deletions(-) 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 00a167ba..95079bc8 100644 --- a/rmf_demos/launch/office.launch.xml +++ b/rmf_demos/launch/office.launch.xml @@ -20,20 +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 @@ + + + + + + + From cbfed0568dae623ccdaeb819a77ffa7814164f7d Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 8 Dec 2022 11:55:35 +0800 Subject: [PATCH 6/7] Port lift adapter changes Signed-off-by: Luca Della Vedova --- rmf_demos_lift_adapter/README.md | 72 +++++++++++++++++++ .../launch/lift_adapter.launch.xml | 19 +++-- .../rmf_demos_lift_adapter/LiftAPI.py | 47 ++++++------ .../rmf_demos_lift_adapter/lift_adapter.py | 45 +++++------- .../rmf_demos_lift_adapter/lift_manager.py | 48 ++++++------- 5 files changed, 147 insertions(+), 84 deletions(-) create mode 100644 rmf_demos_lift_adapter/README.md 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 index 8ca857a9..77641221 100644 --- a/rmf_demos_lift_adapter/launch/lift_adapter.launch.xml +++ b/rmf_demos_lift_adapter/launch/lift_adapter.launch.xml @@ -3,24 +3,21 @@ - + + - - + + + - - + + + diff --git a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py index 2d9794e1..7cbc72ee 100644 --- a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/LiftAPI.py @@ -17,35 +17,30 @@ from __future__ import annotations import requests -from yaml import YAMLObject 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 endpoints - within these functions. -""" +''' + 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, config: YAMLObject, logger: RcutilsLogger): - self.config = config - self.prefix = 'http://' + config['lift_manager']['ip'] +\ - ':' + str(config['lift_manager']['port']) + 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) -> Optional[LiftState]: - """Returns the lift state or None if the query failed.""" + 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}', @@ -65,11 +60,23 @@ def lift_state(self, lift_name) -> Optional[LiftState]: lift_state.destination_floor = res_data['destination_floor'] return lift_state - def command_lift(self, lift_name, 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 - """ + 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 + 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 index 3f336844..3cfefc42 100644 --- a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py @@ -15,10 +15,7 @@ # limitations under the License. import sys -import yaml -import argparse from typing import Optional -from yaml import YAMLObject import rclpy from rclpy.node import Node @@ -27,24 +24,24 @@ 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, args, config: YAMLObject): + def __init__(self): super().__init__('rmf_demos_lift_adapter') self.lift_states = {} self.lift_requests = {} - self.lift_config = config - self.lift_api = LiftAPI(self.lift_config, self.get_logger()) - for lift in config['lifts']: - self.lift_requests[lift] = None - self.lift_states[lift] = self._lift_state(lift) + + 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, @@ -61,7 +58,11 @@ def __init__(self, args, config: YAMLObject): def update_callback(self): new_states = {} - for lift_name, lift_state in self.lift_states.items(): + 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: @@ -75,8 +76,8 @@ def update_callback(self): continue # If all is done, set request to None - if lift_request.destination_floor ==\ - new_state.current_floor and\ + 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 @@ -102,7 +103,7 @@ def _lift_state(self, lift_name) -> Optional[LiftState]: lift_request = self.lift_requests[lift_name] if lift_request is not None: - if lift_request.request_type ==\ + if lift_request.request_type == \ LiftRequest.REQUEST_END_SESSION: new_state.session_id = '' else: @@ -122,7 +123,7 @@ def lift_request_callback(self, msg): return lift_state = self.lift_states[msg.lift_name] - if lift_state is not None and\ + 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)) @@ -139,18 +140,8 @@ def lift_request_callback(self, msg): def main(argv=sys.argv): - args_without_ros = rclpy.utilities.remove_ros_args(argv) - parser = argparse.ArgumentParser( - prog='rmf_demos_lift_adapter', - description='RMF Demos lift adapter') - parser.add_argument('-c', '--config', required=True, type=str) - args = parser.parse_args(args_without_ros[1:]) - - with open(args.config, 'r') as f: - config = yaml.safe_load(f) - rclpy.init() - node = DemoLiftAdapter(args, config) + node = DemoLiftAdapter() rclpy.spin(node) rclpy.shutdown() 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 index 9bcce4fb..f6c26872 100644 --- a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py @@ -17,9 +17,6 @@ import sys import threading -import argparse -import yaml - import rclpy from rclpy.node import Node from rclpy.qos import qos_profile_system_default @@ -55,12 +52,13 @@ class Response(BaseModel): class LiftManager(Node): - def __init__(self, lifts, namespace='sim'): + 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 = {} - for lift in lifts: - self.lift_states[lift] = None # Setup publisher and subscriber self.lift_request_pub = self.create_publisher( @@ -84,13 +82,10 @@ async def state(lift_name: str): } if lift_name not in self.lift_states: - self.get_logger().warn('Lift not being managed') + self.get_logger().warn(f'Lift {lift_name} not being managed') return response state = self.lift_states[lift_name] - if state is None: - self.get_logger().warn('Lift state not received') - return response response['data']['available_floors'] = state.available_floors response['data']['current_floor'] = state.current_floor @@ -100,6 +95,19 @@ async def state(lift_name: str): 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): @@ -111,7 +119,7 @@ async def request(lift_name: str, floor: Request): } if lift_name not in self.lift_states: - self.get_logger().warn('Lift not being managed') + self.get_logger().warn(f'Lift {lift_name} not being managed') return response now = self.get_clock().now() @@ -127,31 +135,19 @@ async def request(lift_name: str, floor: Request): return response def lift_state_cb(self, msg): - if msg.lift_name not in self.lift_states: - return self.lift_states[msg.lift_name] = msg def main(argv=sys.argv): - args_without_ros = rclpy.utilities.remove_ros_args(argv) - parser = argparse.ArgumentParser( - prog='lift_manager', - description='Demo lift manager') - parser.add_argument('-c', '--config', required=True, type=str) - args = parser.parse_args(args_without_ros[1:]) - - with open(args.config, 'r') as f: - config = yaml.safe_load(f) - rclpy.init() - node = LiftManager(config['lifts']) + node = LiftManager() spin_thread = threading.Thread(target=rclpy.spin, args=(node,)) spin_thread.start() uvicorn.run(app, - host=config['lift_manager']['ip'], - port=config['lift_manager']['port'], + host=node.address, + port=node.port, log_level='warning') rclpy.shutdown() From 23fa134fa957f77a7de24da16e674cb808fc9c53 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 8 Dec 2022 12:56:20 +0800 Subject: [PATCH 7/7] Fix session id, remove extra printouts Signed-off-by: Luca Della Vedova --- rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py | 4 ---- rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py | 2 -- rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py | 2 +- 3 files changed, 1 insertion(+), 7 deletions(-) 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 index 3f636dbb..6da2fa2d 100644 --- a/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py +++ b/rmf_demos_door_adapter/rmf_demos_door_adapter/door_adapter.py @@ -70,8 +70,6 @@ def publish_states(self): for door_name in self.doors: door_state = self._door_state(door_name) if door_state is None: - self.get_logger().info('No door state received for door ' - f'{door_name}') continue self.door_state_pub.publish(door_state) @@ -81,11 +79,9 @@ def door_request_callback(self, msg): if msg.requested_mode.value == DoorMode.MODE_OPEN: self.door_api.open_door(msg.door_name) - self.get_logger().info(f'Requested to open door {msg.door_name}') elif msg.requested_mode.value == DoorMode.MODE_CLOSED: self.door_api.close_door(msg.door_name) - self.get_logger().info(f'Requested to close door {msg.door_name}') def main(argv=sys.argv): 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 index 3cfefc42..7633539e 100644 --- a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_adapter.py @@ -134,8 +134,6 @@ def lift_request_callback(self, msg): f'Failed to send lift to {msg.destination_floor}.') return - self.get_logger().info(f'Requested lift {msg.lift_name} ' - f'to {msg.destination_floor}.') self.lift_requests[msg.lift_name] = msg 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 index f6c26872..a3a69c06 100644 --- a/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py +++ b/rmf_demos_lift_adapter/rmf_demos_lift_adapter/lift_manager.py @@ -128,7 +128,7 @@ async def request(lift_name: str, floor: Request): req.request_type = req.REQUEST_AGV_MODE req.door_state = floor.door_state req.destination_floor = floor.floor - req.session_id = req.lift_name + '-' + str(now) + req.session_id = 'rmf_demos_lift_adapter' self.lift_request_pub.publish(req) response['success'] = True