Add demo infrastructure adapters

rmf_demos/launch/clinic.launch.xml

@@ -29,4 +29,18 @@
     </include>
   </group>
 
+  <!-- Demo lift adapter -->
+  <group>
+    <include file="$(find-pkg-share rmf_demos_lift_adapter)/launch/lift_adapter.launch.xml">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+    </include>
+  </group>
+
+  <!-- Demo door adapter -->
+  <group>
+    <include file="$(find-pkg-share rmf_demos_door_adapter)/launch/door_adapter.launch.xml">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+    </include>
+  </group>
+
 </launch>

rmf_demos/launch/hotel.launch.xml

@@ -52,6 +52,20 @@
     </include>
   </group>
 
+  <!-- Demo lift adapter -->
+  <group>
+    <include file="$(find-pkg-share rmf_demos_lift_adapter)/launch/lift_adapter.launch.xml">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+    </include>
+  </group>
+
+  <!-- Demo door adapter -->
+  <group>
+    <include file="$(find-pkg-share rmf_demos_door_adapter)/launch/door_adapter.launch.xml">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+    </include>
+  </group>
+
   <!-- Mock Docker Node, to provide CleanerBotA Fleet Adapter fix cleaning task paths -->
   <group>
     <let name="docking_config_file" value="$(find-pkg-share rmf_demos_tasks)/hotel_cleaner_config.yaml"/>

rmf_demos/launch/office.launch.xml

@@ -20,4 +20,11 @@
     </include>
   </group>
 
+  <!-- Demo door adapter -->
+  <group>
+    <include file="$(find-pkg-share rmf_demos_door_adapter)/launch/door_adapter.launch.xml">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+    </include>
+  </group>
+
 </launch>

rmf_demos/launch/office_mock_traffic_light.launch.xml

@@ -32,4 +32,11 @@
     </include>
   </group>
 
+  <!-- Demo door adapter -->
+  <group>
+    <include file="$(find-pkg-share rmf_demos_door_adapter)/launch/door_adapter.launch.xml">
+      <arg name="use_sim_time" value="$(var use_sim_time)"/>
+    </include>
+  </group>
+
 </launch>

rmf_demos_door_adapter/.gitignore

@@ -0,0 +1 @@
+__pycache__

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": ""
+}
+```

rmf_demos_door_adapter/launch/door_adapter.launch.xml

@@ -0,0 +1,24 @@
+<?xml version='1.0' ?>
+
+<launch>
+
+  <arg name="use_sim_time" default="true" description="Use the /clock topic for time to sync with simulation"/>
+  <arg name="manager_address" default="localhost" description="The address for the manager"/>
+  <arg name="manager_port" default="5002" description="The port for the manager"/>
+  <arg name="output" default="screen"/>
+
+  <!-- Door manager -->
+  <node pkg="rmf_demos_door_adapter" exec="door_manager" output="both">
+    <param name="use_sim_time" value="$(var use_sim_time)"/>
+    <param name="manager_address" value="$(var manager_address)"/>
+    <param name="manager_port" value="$(var manager_port)"/>
+  </node>
+
+  <!-- Door adapter -->
+  <node pkg="rmf_demos_door_adapter" exec="door_adapter" output="both">
+    <param name="use_sim_time" value="$(var use_sim_time)"/>
+    <param name="manager_address" value="$(var manager_address)"/>
+    <param name="manager_port" value="$(var manager_port)"/>
+  </node>
+
+</launch>

rmf_demos_door_adapter/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rmf_demos_door_adapter</name>
+  <version>2.0.2</version>
+  <description>Example door adapter to be used with rmf_demos simulations</description>
+  <maintainer email="[email protected]">Luca Della Vedova</maintainer>
+  <license>Apache 2.0</license>
+
+  <exec_depend>rmf_door_msgs</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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)

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()