From 54b48ad8f044e514e878810d2c2eba5ea6424109 Mon Sep 17 00:00:00 2001 From: Yadu Date: Mon, 29 Jan 2024 19:44:28 +0800 Subject: [PATCH] Backport #208 (#210) * Support fleet-level task (#201) * update task request to include fleet_name for fleet-level task Signed-off-by: Charly Wu * added DCO commit. Signed-off-by: Charly Wu * simplify logic for populating request['fleet_name'] Signed-off-by: Charly Wu * remove extra whitespaces. Signed-off-by: Charly Wu * remove trailing whitespace. Signed-off-by: Charly Wu --------- Signed-off-by: Charly Wu Co-authored-by: Yadunund * Update CI to rolling on main (#208) * Update CI to rolling Signed-off-by: Yadunund * Address all flake8 and pep257 errors Signed-off-by: Yadunund --------- Signed-off-by: Yadunund * Set CI to iron Signed-off-by: Yadunund --------- Signed-off-by: Charly Wu Signed-off-by: Yadunund Co-authored-by: cwrx777 --- .github/workflows/build.yaml | 67 +--- .github/workflows/style.yml | 14 +- .../fleet_robotmanager_mqtt_bridge.py | 210 ++++++----- .../fleet_socketio_bridge.py | 213 ++++++----- .../test/socketio_subscribe.py | 3 +- rmf_demos_bridges/setup.py | 14 +- .../build_configuration.py | 10 +- .../rmf_demos_fleet_adapter/RobotClientAPI.py | 110 +++--- .../rmf_demos_fleet_adapter/fleet_adapter.py | 156 ++++---- .../rmf_demos_fleet_adapter/fleet_manager.py | 221 ++++++----- rmf_demos_fleet_adapter/setup.py | 20 +- rmf_demos_fleet_adapter/test/test_flake8.py | 6 +- .../rmf_demos_panel/dispatcher_client.py | 344 ++++++++++-------- .../rmf_demos_panel/rmf_msg_observer.py | 55 +-- .../rmf_demos_panel/simple_api_server.py | 133 ++++--- rmf_demos_panel/setup.py | 18 +- .../rmf_demos_tasks/cancel_task.py | 37 +- .../rmf_demos_tasks/dispatch_action.py | 177 +++++---- .../rmf_demos_tasks/dispatch_clean.py | 151 ++++---- .../rmf_demos_tasks/dispatch_delivery.py | 248 ++++++++----- .../rmf_demos_tasks/dispatch_go_to_place.py | 90 +++-- .../rmf_demos_tasks/dispatch_loop.py | 78 ++-- .../rmf_demos_tasks/dispatch_patrol.py | 122 ++++--- .../rmf_demos_tasks/dispatch_teleop.py | 131 ++++--- .../rmf_demos_tasks/mock_docker.py | 82 +++-- .../rmf_demos_tasks/request_lift.py | 25 +- .../rmf_demos_tasks/request_loop.py | 37 +- .../rmf_demos_tasks/teleop_robot.py | 50 ++- rmf_demos_tasks/setup.py | 30 +- 29 files changed, 1614 insertions(+), 1238 deletions(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index d2f48410..1a6a0bef 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -1,53 +1,24 @@ name: build on: pull_request: + schedule: + - cron: '0 0 * * *' + workflow_dispatch: jobs: - build: - runs-on: ubuntu-latest - strategy: - matrix: - docker_image: ['ros:humble-ros-base'] - container: - image: ${{ matrix.docker_image }} - steps: - - name: non-ros-deps - run: | - sudo apt update - sudo apt install -y wget - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' - wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - - sudo apt update && sudo apt install \ - git cmake python3-vcstool curl \ - clang clang-tools lld \ - python3-shapely python3-yaml python3-requests \ - python3-pip -y - pip3 install flask-socketio fastapi uvicorn - - name: create-ws - run: | - mkdir -p rmf_demos_ws/src - - name: checkout # This will check out the current rmf_demos branch for build tests - uses: actions/checkout@v2 - with: - path: rmf_demos_ws/src/demonstrations/rmf_demos/ - - name: workspace - run: | - cd rmf_demos_ws - wget https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos - vcs import src < rmf.repos --skip-existing - - name: ros-deps - run: | - export DEBIAN_FRONTEND=noninteractive - colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - colcon mixin update default - cd rmf_demos_ws - rosdep update - rosdep install --from-paths src --ignore-src --rosdistro humble -yr - - name: build - shell: bash - run: | - cd rmf_demos_ws - source /opt/ros/humble/setup.bash - export CXX=clang++ - export CX=clang - colcon build --mixin lld --cmake-args -DNO_DOWNLOAD_MODELS=True + build_and_test: + name: rmf_demos + uses: open-rmf/rmf_ci_templates/.github/workflows/reusable_build.yaml@main + with: + packages: | + rmf_demos + rmf_demos_assets + rmf_demos_bridges + rmf_demos_dashboard_resources + rmf_demos_fleet_adapter + rmf_demos_gz + rmf_demos_gz_classic + rmf_demos_maps + rmf_demos_panel + rmf_demos_tasks + dist-matrix: '[{"ros_distribution": "iron", "ubuntu_distribution": "jammy"}]' diff --git a/.github/workflows/style.yml b/.github/workflows/style.yml index f91e9ddb..10904913 100644 --- a/.github/workflows/style.yml +++ b/.github/workflows/style.yml @@ -8,23 +8,15 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - docker_image: ['ros:humble-ros-base'] + docker_image: ['ros:iron-ros-base'] container: image: ${{ matrix.docker_image }} steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: deps shell: bash run: | - sudo apt-get update - sudo apt-get install wget pycodestyle - # Note: Enable this if only cpp files are available for style check - # - name: rmf_uncrustify - # shell: bash - # run: | - # wget https://raw.githubusercontent.com/open-rmf/rmf_utils/master/rmf_utils/test/format/rmf_code_style.cfg - # source /opt/ros/foxy/setup.bash - # ament_uncrustify -c rmf_code_style.cfg + sudo apt update && sudo apt install pycodestyle - name: pycodestyle shell: bash run: | diff --git a/rmf_demos_bridges/rmf_demos_bridges/fleet_robotmanager_mqtt_bridge.py b/rmf_demos_bridges/rmf_demos_bridges/fleet_robotmanager_mqtt_bridge.py index 5ceb3c52..ca893462 100644 --- a/rmf_demos_bridges/rmf_demos_bridges/fleet_robotmanager_mqtt_bridge.py +++ b/rmf_demos_bridges/rmf_demos_bridges/fleet_robotmanager_mqtt_bridge.py @@ -13,96 +13,112 @@ # limitations under the License. -from flask_cors import CORS -from flask import Flask, request, jsonify -import rclpy -import rclpy.executors -import threading -import sys -import json -import copy import argparse from collections import OrderedDict -from rosidl_runtime_py import message_to_ordereddict -from pyproj import Transformer +import copy +import json +import sys +import threading +from flask import Flask +from flask import jsonify +from flask import request +from flask_cors import CORS +from flask_socketio import disconnect +from flask_socketio import emit +from flask_socketio import SocketIO +import paho.mqtt.client as mqtt +from pyproj import Transformer +import rclpy +import rclpy.executors from rclpy.node import Node -from flask_socketio import SocketIO, emit, disconnect - -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_fleet_msgs.msg import FleetState, RobotState -import paho.mqtt.client as mqtt +from rmf_fleet_msgs.msg import FleetState +from rmf_fleet_msgs.msg import RobotState +from rosidl_runtime_py import message_to_ordereddict SUPPORTED_GPS_FRAMES = ['svy21'] ROBOT_ID_TO_AUTHKEY_MAP = { - "deliveryRobot_1": "00000000-0000-0000-0000-000000000001", - "deliveryRobot_2": "00000000-0000-0000-0000-000000000002", - "deliveryRobot_3": "00000000-0000-0000-0000-000000000003", + 'deliveryRobot_1': '00000000-0000-0000-0000-000000000001', + 'deliveryRobot_2': '00000000-0000-0000-0000-000000000002', + 'deliveryRobot_3': '00000000-0000-0000-0000-000000000003', } ROBOTMANAGER_GPS_DEFINITION = { - "batteryPct": 100.0, - "mapPose": { - "x": 1076.0, - "y": 456.0, - "z": 0.0, - "heading": 0.0 + 'batteryPct': 100.0, + 'mapPose': {'x': 1076.0, 'y': 456.0, 'z': 0.0, 'heading': 0.0}, + 'globalPose': { + 'lat': 1.30032, + 'lng': 103.78063, + 'alt': 0.0, + 'heading': 0.0, }, - "globalPose": { - "lat": 1.30032, - "lng": 103.78063, - "alt": 0.0, - "heading": 0.0 - }, - "state": 3 + 'state': 3, } class FleetRobotManagerMQTTBridge(Node): def __init__(self, argv=sys.argv): parser = argparse.ArgumentParser() - parser.add_argument('-t', '--robot_state_topic', - required=False, - type=str, - default='/robot_state', - help='Topic to listen on for Robot States') - parser.add_argument('-f', '--filter_fleet', - required=False, - type=str, - help='Only listen to this fleet.') - parser.add_argument('-g', '--mqtt_base_topic', - required=False, - type=str, - default="/robot/status/", - help='MQTT base topic to publish GPS.') - parser.add_argument('-x', '--offset_x', - required=False, - type=float, - default=0.0, - help='X offset for simulations.') - parser.add_argument('-y', '--offset_y', - required=False, - type=float, - default=0.0, - help='Y offset for simulations.') - parser.add_argument('-m', '--mqtt_server', - required=False, - type=str, - default="localhost", - help='MQTT server url') + parser.add_argument( + '-t', + '--robot_state_topic', + required=False, + type=str, + default='/robot_state', + help='Topic to listen on for Robot States', + ) + parser.add_argument( + '-f', + '--filter_fleet', + required=False, + type=str, + help='Only listen to this fleet.', + ) + parser.add_argument( + '-g', + '--mqtt_base_topic', + required=False, + type=str, + default='/robot/status/', + help='MQTT base topic to publish GPS.', + ) + parser.add_argument( + '-x', + '--offset_x', + required=False, + type=float, + default=0.0, + help='X offset for simulations.', + ) + parser.add_argument( + '-y', + '--offset_y', + required=False, + type=float, + default=0.0, + help='Y offset for simulations.', + ) + parser.add_argument( + '-m', + '--mqtt_server', + required=False, + type=str, + default='localhost', + help='MQTT server url', + ) self.args = parser.parse_args(argv[1:]) - super().__init__(f"fleet_robotmanager_mqtt_bridge") + super().__init__(f'fleet_robotmanager_mqtt_bridge') self._init_pubsub() - self.mqtt_pubs = {} # Map of robot names to their mqtt Client + self.mqtt_pubs = {} # Map of robot names to their mqtt Client self._init_mqtt() self._init_gps_conversion_tools('svy21') @@ -113,14 +129,12 @@ def robot_state_callback(self, msg: RobotState): return robot = msg if robot.name not in ROBOT_ID_TO_AUTHKEY_MAP.keys(): - print( - f"Skipping {robot.name} as it does not have auth key") + print(f'Skipping {robot.name} as it does not have auth key') else: json = self._robot_state_to_gps_json(robot) rbmgr_uuid = ROBOT_ID_TO_AUTHKEY_MAP[robot.name] self.mqtt_pubs[robot.name].publish( - self.args.mqtt_base_topic + rbmgr_uuid, - json + self.args.mqtt_base_topic + rbmgr_uuid, json ) except Exception as e: @@ -131,61 +145,62 @@ def _init_pubsub(self): RobotState, self.args.robot_state_topic, self.robot_state_callback, - 10) + 10, + ) def _init_mqtt(self): try: for robot_name in ROBOT_ID_TO_AUTHKEY_MAP.keys(): self.mqtt_pubs[robot_name] = mqtt.Client( - 'rbmgr_pub_' + ROBOT_ID_TO_AUTHKEY_MAP[robot_name]) + 'rbmgr_pub_' + ROBOT_ID_TO_AUTHKEY_MAP[robot_name] + ) self.mqtt_pubs[robot_name].connect(self.args.mqtt_server) except ConnectionRefusedError as e: - print(f"MQTT connection to {self.args.mqtt_server} failed.") - print("Please check that the MQTT server is running!") - raise(e) + print(f'MQTT connection to {self.args.mqtt_server} failed.') + print('Please check that the MQTT server is running!') + raise (e) def _init_gps_conversion_tools(self, frame: str): assert frame in SUPPORTED_GPS_FRAMES if frame == 'svy21': self._wgs_transformer = Transformer.from_crs( - 'EPSG:3414', 'EPSG:4326') + 'EPSG:3414', 'EPSG:4326' + ) return - raise Exception("This should not happen") + raise Exception('This should not happen') def _robot_state_to_gps_json(self, robot_state: RobotState): resp = copy.deepcopy(ROBOTMANAGER_GPS_DEFINITION) svy21_xy = self._remove_offsets( - robot_state.location.x, - robot_state.location.y + robot_state.location.x, robot_state.location.y ) wgs84_xy = self._wgs_transformer.transform( - svy21_xy[1], svy21_xy[0]) # inputs are y,x - resp["timestamp"] = robot_state.location.t.sec - resp["robot_id"] = robot_state.name - resp["globalPose"]["lat"] = wgs84_xy[0] - resp["globalPose"]["lng"] = wgs84_xy[1] - resp["globalPose"]["alt"] = 0 - resp["globalPose"]["heading"] = robot_state.location.yaw - - resp["mapPose"]["x"] = robot_state.location.x - resp["mapPose"]["y"] = robot_state.location.y - resp["mapPose"]["z"] = 0.0 - resp["mapPose"]["heading"] = robot_state.location.yaw - - resp["batteryPct"] = robot_state.battery_percent + svy21_xy[1], svy21_xy[0] + ) # inputs are y,x + resp['timestamp'] = robot_state.location.t.sec + resp['robot_id'] = robot_state.name + resp['globalPose']['lat'] = wgs84_xy[0] + resp['globalPose']['lng'] = wgs84_xy[1] + resp['globalPose']['alt'] = 0 + resp['globalPose']['heading'] = robot_state.location.yaw + + resp['mapPose']['x'] = robot_state.location.x + resp['mapPose']['y'] = robot_state.location.y + resp['mapPose']['z'] = 0.0 + resp['mapPose']['heading'] = robot_state.location.yaw + + resp['batteryPct'] = robot_state.battery_percent return json.dumps(resp) def _remove_offsets(self, x: float, y: float): - return (x + self.args.offset_x, - y + self.args.offset_y) + return (x + self.args.offset_x, y + self.args.offset_y) def _apply_offsets(self, x: float, y: float): - return (x - self.args.offset_x, - y - self.args.offset_y) + return (x - self.args.offset_x, y - self.args.offset_y) def main(argv=sys.argv): @@ -198,13 +213,12 @@ def main(argv=sys.argv): except KeyboardInterrupt: pass except OSError: - node.get_logger().error( - "Check if target port is already in use.") + node.get_logger().error('Check if target port is already in use.') finally: node.get_logger().info('shutting down...') node.destroy_node() rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main(sys.argv) diff --git a/rmf_demos_bridges/rmf_demos_bridges/fleet_socketio_bridge.py b/rmf_demos_bridges/rmf_demos_bridges/fleet_socketio_bridge.py index 5521b264..0294dd9d 100644 --- a/rmf_demos_bridges/rmf_demos_bridges/fleet_socketio_bridge.py +++ b/rmf_demos_bridges/rmf_demos_bridges/fleet_socketio_bridge.py @@ -13,29 +13,32 @@ # limitations under the License. -from flask_cors import CORS -from flask import Flask, request, jsonify -import rclpy -import rclpy.executors -import threading -import sys -import json -import copy import argparse -import logging from collections import OrderedDict -from rosidl_runtime_py import message_to_ordereddict -from pyproj import Transformer +import copy +import json +import logging +import sys +import threading +from flask import Flask +from flask import jsonify +from flask import request +from flask_cors import CORS +from flask_socketio import disconnect +from flask_socketio import emit +from flask_socketio import SocketIO +from pyproj import Transformer +import rclpy +import rclpy.executors from rclpy.node import Node -from flask_socketio import SocketIO, emit, disconnect - -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_fleet_msgs.msg import FleetState, RobotState +from rmf_fleet_msgs.msg import FleetState +from rmf_fleet_msgs.msg import RobotState +from rosidl_runtime_py import message_to_ordereddict log = logging.getLogger('werkzeug') log.disabled = True @@ -44,61 +47,80 @@ # Temporary Message definition for GPS messages GPS_MESSAGE_DEFINITION = { - "timestamp": -1, # Unix time - "robot_id": "", # String - "lat": -1, # Float32 - "lon": -1, # Float32 - "alt": -1, # Float32 - "heading": 0, # Uint - - "x": 0, # 2D robot x position - "y": 0, # 2D robot y position - "angle": 0, # 2D robot angle - - "battery": 100.0, # Battery percentage + 'timestamp': -1, # Unix time + 'robot_id': '', # String + 'lat': -1, # Float32 + 'lon': -1, # Float32 + 'alt': -1, # Float32 + 'heading': 0, # Uint + 'x': 0, # 2D robot x position + 'y': 0, # 2D robot y position + 'angle': 0, # 2D robot angle + 'battery': 100.0, # Battery percentage } class FleetSocketIOBridge(Node): def __init__(self, argv=sys.argv): parser = argparse.ArgumentParser() - parser.add_argument('-t', '--robot_state_topic', - required=False, - type=str, - default='/robot_state', - help='Topic to listen on for Robot States') - parser.add_argument('-f', '--filter_fleet', - required=False, - type=str, - help='Only listen to this fleet.') - parser.add_argument('-g', '--gps_state_topic', - required=False, - type=str, - help='SocketIO topic to publish GPS.') - parser.add_argument('-x', '--offset_x', - required=False, - type=float, - default=0.0, - help='X offset for simulations.') - parser.add_argument('-y', '--offset_y', - required=False, - type=float, - default=0.0, - help='Y offset for simulations.') - parser.add_argument('-i', '--listening_interfaces', - required=False, - type=str, - default='0.0.0.0', - help='Interfaces for SocketIO to serve on') - parser.add_argument('-p', '--listening_port', - required=False, - type=str, - default='8080', - help='Port for SocketIO to serve on') + parser.add_argument( + '-t', + '--robot_state_topic', + required=False, + type=str, + default='/robot_state', + help='Topic to listen on for Robot States', + ) + parser.add_argument( + '-f', + '--filter_fleet', + required=False, + type=str, + help='Only listen to this fleet.', + ) + parser.add_argument( + '-g', + '--gps_state_topic', + required=False, + type=str, + help='SocketIO topic to publish GPS.', + ) + parser.add_argument( + '-x', + '--offset_x', + required=False, + type=float, + default=0.0, + help='X offset for simulations.', + ) + parser.add_argument( + '-y', + '--offset_y', + required=False, + type=float, + default=0.0, + help='Y offset for simulations.', + ) + parser.add_argument( + '-i', + '--listening_interfaces', + required=False, + type=str, + default='0.0.0.0', + help='Interfaces for SocketIO to serve on', + ) + parser.add_argument( + '-p', + '--listening_port', + required=False, + type=str, + default='8080', + help='Port for SocketIO to serve on', + ) self.args = parser.parse_args(argv[1:]) - super().__init__(f"fleet_socketio_bridge") + super().__init__(f'fleet_socketio_bridge') self._init_pubsub() self._init_webserver() @@ -112,13 +134,13 @@ def robot_state_callback(self, msg: RobotState): if self.args.filter_fleet not in msg.name: return - self._sio.emit(self.args.robot_state_topic, - message_to_ordereddict(msg)) + self._sio.emit( + self.args.robot_state_topic, message_to_ordereddict(msg) + ) if self.args.gps_state_topic: robot_state_json = self._robot_state_to_gps_json(msg) - self._sio.emit(self.args.gps_state_topic, - robot_state_json) + self._sio.emit(self.args.gps_state_topic, robot_state_json) except Exception as e: print(e) @@ -127,71 +149,73 @@ def start_socketio(self): def spin_background(self): def spin(): - self.get_logger().info("start spinning rclpy node") + self.get_logger().info('start spinning rclpy node') rclpy.spin_until_future_complete(self, self._finish_spin) - self.get_logger().info("Finished spinning") + self.get_logger().info('Finished spinning') + self._spin_thread = threading.Thread(target=spin) self._spin_thread.start() def _init_webserver(self): self._finish_spin = rclpy.executors.Future() self._finish_gc = self.create_guard_condition( - lambda: self._finish_spin.set_result(None)) + lambda: self._finish_spin.set_result(None) + ) self._app = Flask(__name__) - CORS(self._app, origins=r"/*") + CORS(self._app, origins=r'/*') self._sio = SocketIO(self._app, async_mode='threading') - self._sio.init_app(self._app, cors_allowed_origins="*") + self._sio.init_app(self._app, cors_allowed_origins='*') def _init_pubsub(self): self.robot_state_sub = self.create_subscription( RobotState, self.args.robot_state_topic, self.robot_state_callback, - 10) + 10, + ) def _init_gps_conversion_tools(self, frame: str): assert frame in SUPPORTED_GPS_FRAMES if frame == 'svy21': self._wgs_transformer = Transformer.from_crs( - 'EPSG:3414', 'EPSG:4326') + 'EPSG:3414', 'EPSG:4326' + ) return - raise Exception("This should not happen") + raise Exception('This should not happen') def _robot_state_to_gps_json(self, robot_state: RobotState): resp = copy.deepcopy(GPS_MESSAGE_DEFINITION) svy21_xy = self._remove_offsets( - robot_state.location.x, - robot_state.location.y + robot_state.location.x, robot_state.location.y ) wgs84_xy = self._wgs_transformer.transform( - svy21_xy[1], svy21_xy[0]) # inputs are y,x - resp["timestamp"] = robot_state.location.t.sec - resp["robot_id"] = robot_state.name - resp["lat"] = wgs84_xy[0] - resp["lon"] = wgs84_xy[1] - resp["alt"] = 0 # BH(WARN): Hardcoded - resp["heading"] = robot_state.location.yaw + svy21_xy[1], svy21_xy[0] + ) # inputs are y,x + resp['timestamp'] = robot_state.location.t.sec + resp['robot_id'] = robot_state.name + resp['lat'] = wgs84_xy[0] + resp['lon'] = wgs84_xy[1] + resp['alt'] = 0 # BH(WARN): Hardcoded + resp['heading'] = robot_state.location.yaw - resp["x"] = robot_state.location.x - resp["y"] = robot_state.location.y - resp["angle"] = robot_state.location.yaw + resp['x'] = robot_state.location.x + resp['y'] = robot_state.location.y + resp['angle'] = robot_state.location.yaw - resp["battery"] = robot_state.battery_percent + resp['battery'] = robot_state.battery_percent return json.dumps(resp) def _remove_offsets(self, x: float, y: float): - return (x + self.args.offset_x, - y + self.args.offset_y) + return (x + self.args.offset_x, y + self.args.offset_y) def _apply_offsets(self, x: float, y: float): - return (x - self.args.offset_x, - y - self.args.offset_y) + return (x - self.args.offset_x, y - self.args.offset_y) def main(argv=sys.argv): @@ -205,13 +229,12 @@ def main(argv=sys.argv): except KeyboardInterrupt: pass except OSError: - node.get_logger().error( - "Check if target port is already in use.") + node.get_logger().error('Check if target port is already in use.') finally: node.get_logger().info('shutting down...') node.destroy_node() rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main(sys.argv) diff --git a/rmf_demos_bridges/rmf_demos_bridges/test/socketio_subscribe.py b/rmf_demos_bridges/rmf_demos_bridges/test/socketio_subscribe.py index 33a915ae..94b4a4ee 100644 --- a/rmf_demos_bridges/rmf_demos_bridges/test/socketio_subscribe.py +++ b/rmf_demos_bridges/rmf_demos_bridges/test/socketio_subscribe.py @@ -1,6 +1,7 @@ -import socketio import sys +import socketio + sio = socketio.Client() topic = '/fleet_states' diff --git a/rmf_demos_bridges/setup.py b/rmf_demos_bridges/setup.py index fcd33469..03b6f677 100644 --- a/rmf_demos_bridges/setup.py +++ b/rmf_demos_bridges/setup.py @@ -7,8 +7,10 @@ version='2.1.2', packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ( + 'share/ament_index/resource_index/packages', + ['resource/' + package_name], + ), ('share/' + package_name, ['package.xml']), ], install_requires=['setuptools'], @@ -20,10 +22,12 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - ('fleet_socketio_bridge=' + ( + 'fleet_socketio_bridge=' 'rmf_demos_bridges.fleet_socketio_bridge:main', - 'fleet_robotmanager_mqtt_bridge=' - 'rmf_demos_bridges.fleet_robotmanager_mqtt_bridge:main'), + 'fleet_robotmanager_mqtt_bridge=' + 'rmf_demos_bridges.fleet_robotmanager_mqtt_bridge:main', + ), ], }, ) diff --git a/rmf_demos_dashboard_resources/build_configuration.py b/rmf_demos_dashboard_resources/build_configuration.py index 65ec7b2b..0affdc10 100644 --- a/rmf_demos_dashboard_resources/build_configuration.py +++ b/rmf_demos_dashboard_resources/build_configuration.py @@ -1,20 +1,16 @@ import glob -import os import json +import os from pathlib import Path # Get the directories names inside the folder rmf_dashboard_resources worlds = next(os.walk('.'))[1] for world in worlds: - app_config = { - "dispensers": {}, - "robots": {}, - "logos": {} - } + app_config = {'dispensers': {}, 'robots': {}, 'logos': {}} for topic in app_config.keys(): # Get all JSON files inside the folder of each world - files = Path(world + '/' + topic + '/').glob("**/*.json") + files = Path(world + '/' + topic + '/').glob('**/*.json') for file in files: with open(file) as json_file: app_config[topic].update(json.load(json_file)) diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py index 39205074..24792be9 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/RobotClientAPI.py @@ -13,18 +13,19 @@ # limitations under the License. -''' - The RobotAPI class is a wrapper for API calls to the robot. Here users - are expected to fill up the implementations of functions which will be used - by the RobotCommandHandle. For example, if your robot has a REST API, you - will need to make http request calls to the appropriate endpoints within - these functions. -''' - -import requests +""" +The RobotAPI class is a wrapper for API calls to the robot. + +Here users are expected to fill up the implementations of functions which will +be used by the RobotCommandHandle. For example, if your robot has a REST API, +you will need to make http request calls to the appropriate endpoints within +these functions. +""" import enum from urllib.error import HTTPError +import requests + class RobotAPIResult(enum.IntEnum): SUCCESS = 0 @@ -49,7 +50,7 @@ def __init__(self, prefix: str, user: str, password: str): self.debug = False def check_connection(self): - ''' Return True if connection to the robot API server is successful''' + """Return True if connection to the robot API server is successful.""" if self.data() is None: return False return True @@ -60,16 +61,21 @@ def navigate( cmd_id: int, pose, map_name: str, - speed_limit=0.0 + speed_limit=0.0, ): - ''' Request the robot to navigate to pose:[x,y,theta] where x, y and - and theta are in the robot's coordinate convention. This function - should return True if the robot has accepted the request, - else False''' - assert(len(pose) > 2) - url = self.prefix +\ - f'/open-rmf/rmf_demos_fm/navigate?robot_name={robot_name}' \ + """ + Request the robot to navigate to pose:[x,y,theta]. + + Where x, y and theta are in the robot's coordinate convention. + This function should return True if the robot has accepted the request, + else False. + """ + assert len(pose) > 2 + url = ( + self.prefix + + f'/open-rmf/rmf_demos_fm/navigate?robot_name={robot_name}' f'&cmd_id={cmd_id}' + ) data = {} # data fields: task, map_name, destination{}, data{} data['map_name'] = map_name data['destination'] = {'x': pose[0], 'y': pose[1], 'yaw': pose[2]} @@ -87,19 +93,19 @@ def navigate( return False def start_activity( - self, - robot_name: str, - cmd_id: int, - activity: str, - label: str + self, robot_name: str, cmd_id: int, activity: str, label: str ): - ''' Request the robot to begin a process. This is specific to the robot - and the use case. For example, load/unload a cart for Deliverybot - or begin cleaning a zone for a cleaning robot.''' + """ + Request the robot to begin a process. + + This is specific to the robot and the use case. + For example, load/unload a cart for Deliverybot + or begin cleaning a zone for a cleaning robot. + """ url = ( - self.prefix + - f"/open-rmf/rmf_demos_fm/start_activity?robot_name={robot_name}" - f"&cmd_id={cmd_id}" + self.prefix + + f'/open-rmf/rmf_demos_fm/start_activity?robot_name={robot_name}' + f'&cmd_id={cmd_id}' ) # data fields: task, map_name, destination{}, data{} data = {'activity': activity, 'label': label} @@ -112,7 +118,7 @@ def start_activity( if response.json()['success']: return ( RobotAPIResult.SUCCESS, - response.json()['data']['path'] + response.json()['data']['path'], ) # If we get a response with success=False, then @@ -124,11 +130,16 @@ def start_activity( return RobotAPIResult.RETRY def stop(self, robot_name: str, cmd_id: int): - ''' Command the robot to stop. - Return True if robot has successfully stopped. Else False''' - url = self.prefix +\ - f'/open-rmf/rmf_demos_fm/stop_robot?robot_name={robot_name}' \ + """ + Command the robot to stop. + + Return True if robot has successfully stopped. Else False + """ + url = ( + self.prefix + + f'/open-rmf/rmf_demos_fm/stop_robot?robot_name={robot_name}' f'&cmd_id={cmd_id}' + ) try: response = requests.get(url, self.timeout) response.raise_for_status() @@ -142,10 +153,15 @@ def stop(self, robot_name: str, cmd_id: int): return False def toggle_teleop(self, robot_name: str, toggle: bool): - '''Request to toggle the robot's mode_teleop parameter. - Return True if the toggle request is successful''' - url = self.prefix +\ - f"/open-rmf/rmf_demos_fm/toggle_teleop?robot_name={robot_name}" + """ + Request to toggle the robot's mode_teleop parameter. + + Return True if the toggle request is successful + """ + url = ( + self.prefix + + f'/open-rmf/rmf_demos_fm/toggle_teleop?robot_name={robot_name}' + ) data = {'toggle': toggle} try: response = requests.post(url, timeout=self.timeout, json=data) @@ -161,14 +177,17 @@ def toggle_teleop(self, robot_name: str, toggle: bool): def get_data(self, robot_name: str | None = None): """ - Return a RobotUpdateData for one robot if a name is given. Otherwise - return a list of RobotUpdateData for all robots. + Return a RobotUpdateData for one robot if a name is given. + + Otherwise return a list of RobotUpdateData for all robots. """ if robot_name is None: - url = self.prefix + f'/open-rmf/rmf_demos_fm/status' + url = self.prefix + '/open-rmf/rmf_demos_fm/status' else: - url = self.prefix +\ - f'/open-rmf/rmf_demos_fm/status?robot_name={robot_name}' + url = ( + self.prefix + + f'/open-rmf/rmf_demos_fm/status?robot_name={robot_name}' + ) try: response = requests.get(url, timeout=self.timeout) response.raise_for_status() @@ -189,7 +208,8 @@ def get_data(self, robot_name: str | None = None): class RobotUpdateData: - """Update data for a single robot""" + """Update data for a single robot.""" + def __init__(self, data): self.robot_name = data['robot_name'] position = data['position'] @@ -198,7 +218,7 @@ def __init__(self, data): yaw = position['yaw'] self.position = [x, y, yaw] self.map = data['map_name'] - self.battery_soc = data['battery']/100.0 + self.battery_soc = data['battery'] / 100.0 self.requires_replan = data.get('replan', False) self.last_request_completed = data['last_completed_request'] diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py index af1df33c..6960f523 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_adapter.py @@ -12,32 +12,34 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys import argparse -import yaml -import time -import threading import asyncio import math +import sys +import threading +import time import rclpy +from rclpy.duration import Duration import rclpy.node from rclpy.parameter import Parameter -from rclpy.duration import Duration - +from rclpy.qos import qos_profile_system_default +from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy as Reliability import rmf_adapter from rmf_adapter import Adapter import rmf_adapter.easy_full_control as rmf_easy +from rmf_fleet_msgs.msg import ClosedLanes +from rmf_fleet_msgs.msg import LaneRequest +from rmf_fleet_msgs.msg import ModeRequest +from rmf_fleet_msgs.msg import RobotMode +import yaml -from rmf_fleet_msgs.msg import LaneRequest, ClosedLanes, ModeRequest, RobotMode - -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History -from rclpy.qos import QoSDurabilityPolicy as Durability -from rclpy.qos import QoSReliabilityPolicy as Reliability -from rclpy.qos import qos_profile_system_default - -from .RobotClientAPI import RobotAPI, RobotUpdateData, RobotAPIResult +from .RobotClientAPI import RobotAPI +from .RobotClientAPI import RobotAPIResult +from .RobotClientAPI import RobotUpdateData # ------------------------------------------------------------------------------ @@ -50,16 +52,31 @@ def main(argv=sys.argv): args_without_ros = rclpy.utilities.remove_ros_args(argv) parser = argparse.ArgumentParser( - prog="fleet_adapter", - description="Configure and spin up the fleet adapter") - parser.add_argument("-c", "--config_file", type=str, required=True, - help="Path to the config.yaml file") - parser.add_argument("-n", "--nav_graph", type=str, required=True, - help="Path to the nav_graph for this fleet adapter") - parser.add_argument("-sim", "--use_sim_time", action="store_true", - help='Use sim time, default: false') + prog='fleet_adapter', + description='Configure and spin up the fleet adapter', + ) + parser.add_argument( + '-c', + '--config_file', + type=str, + required=True, + help='Path to the config.yaml file', + ) + parser.add_argument( + '-n', + '--nav_graph', + type=str, + required=True, + help='Path to the nav_graph for this fleet adapter', + ) + parser.add_argument( + '-sim', + '--use_sim_time', + action='store_true', + help='Use sim time, default: false', + ) args = parser.parse_args(args_without_ros[1:]) - print(f"Starting fleet adapter...") + print('Starting fleet adapter...') config_path = args.config_file nav_graph_path = args.nav_graph @@ -70,7 +87,7 @@ def main(argv=sys.argv): assert fleet_config, f'Failed to parse config file [{config_path}]' # Parse the yaml in Python to get the fleet_manager info - with open(config_path, "r") as f: + with open(config_path, 'r') as f: config_yaml = yaml.safe_load(f) # ROS 2 node for the command handle @@ -84,7 +101,7 @@ def main(argv=sys.argv): # Enable sim time for testing offline if args.use_sim_time: - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + param = Parameter('use_sim_time', Parameter.Type.BOOL, True) node.set_parameters([param]) adapter.node.use_sim_time() @@ -92,9 +109,9 @@ def main(argv=sys.argv): time.sleep(1.0) node.declare_parameter('server_uri', '') - server_uri = node.get_parameter( - 'server_uri' - ).get_parameter_value().string_value + server_uri = ( + node.get_parameter('server_uri').get_parameter_value().string_value + ) if server_uri == '': server_uri = None @@ -103,17 +120,14 @@ def main(argv=sys.argv): # Initialize robot API for this fleet fleet_mgr_yaml = config_yaml['fleet_manager'] - update_period = 1.0/fleet_mgr_yaml.get( + update_period = 1.0 / fleet_mgr_yaml.get( 'robot_state_update_frequency', 10.0 ) api_prefix = ( - 'http://' + fleet_mgr_yaml['ip'] + ':' - + str(fleet_mgr_yaml['port']) + 'http://' + fleet_mgr_yaml['ip'] + ':' + str(fleet_mgr_yaml['port']) ) api = RobotAPI( - api_prefix, - fleet_mgr_yaml['user'], - fleet_mgr_yaml['password'] + api_prefix, fleet_mgr_yaml['user'], fleet_mgr_yaml['password'] ) robots = {} @@ -137,7 +151,7 @@ def update_loop(): asyncio.wait(update_jobs) ) - next_wakeup = now + Duration(nanoseconds=update_period*1e9) + next_wakeup = now + Duration(nanoseconds=update_period * 1e9) while node.get_clock().now() < next_wakeup: time.sleep(0.001) @@ -162,12 +176,7 @@ def update_loop(): class RobotAdapter: def __init__( - self, - name: str, - configuration, - node, - api: RobotAPI, - fleet_handle + self, name: str, configuration, node, api: RobotAPI, fleet_handle ): self.name = name self.execution = None @@ -205,7 +214,7 @@ def make_callbacks(self): lambda activity: self.stop(activity), lambda category, description, execution: self.execute_action( category, description, execution - ) + ), ) def navigate(self, destination, execution): @@ -218,8 +227,7 @@ def navigate(self, destination, execution): if destination.dock is not None: self.attempt_cmd_until_success( - cmd=self.perform_docking, - args=(destination,) + cmd=self.perform_docking, args=(destination,) ) return @@ -230,8 +238,8 @@ def navigate(self, destination, execution): self.cmd_id, destination.position, destination.map, - destination.speed_limit - ) + destination.speed_limit, + ), ) def stop(self, activity): @@ -239,8 +247,7 @@ def stop(self, activity): if self.execution.identifier.is_same(activity): self.execution = None self.attempt_cmd_until_success( - cmd=self.api.stop, - args=(self.name, self.cmd_id) + cmd=self.api.stop, args=(self.name, self.cmd_id) ) def execute_action(self, category: str, description: dict, execution): @@ -251,13 +258,11 @@ def execute_action(self, category: str, description: dict, execution): case 'teleop': self.teleoperation = Teleoperation(execution) self.attempt_cmd_until_success( - cmd=self.api.toggle_teleop, - args=(self.name, True) + cmd=self.api.toggle_teleop, args=(self.name, True) ) case 'clean': self.attempt_cmd_until_success( - cmd=self.perform_clean, - args=(description['zone'],) + cmd=self.perform_clean, args=(description['zone'],) ) def finish_action(self): @@ -268,21 +273,16 @@ def finish_action(self): self.execution.finished() self.execution = None self.attempt_cmd_until_success( - cmd=self.api.toggle_teleop, - args=(self.name, False) + cmd=self.api.toggle_teleop, args=(self.name, False) ) def perform_docking(self, destination): match self.api.start_activity( - self.name, - self.cmd_id, - 'dock', - destination.dock() + self.name, self.cmd_id, 'dock', destination.dock() ): case (RobotAPIResult.SUCCESS, path): self.override = self.execution.override_schedule( - path['map_name'], - path['path'] + path['map_name'], path['path'] ) return True case RobotAPIResult.RETRY: @@ -295,7 +295,7 @@ def perform_docking(self, destination): self.cmd_id, destination.position, destination.map, - destination.speed_limit + destination.speed_limit, ) def perform_clean(self, zone): @@ -305,8 +305,7 @@ def perform_clean(self, zone): f'Commanding [{self.name}] to clean zone [{zone}]' ) self.override = self.execution.override_schedule( - path['map_name'], - path['path'] + path['map_name'], path['path'] ) return True case RobotAPIResult.RETRY: @@ -331,10 +330,7 @@ def loop(): if self.cancel_cmd_event.wait(1.0): break - self.issue_cmd_thread = threading.Thread( - target=loop, - args=() - ) + self.issue_cmd_thread = threading.Thread(target=loop, args=()) self.issue_cmd_thread.start() def cancel_cmd_attempt(self): @@ -347,6 +343,7 @@ def cancel_cmd_attempt(self): class Teleoperation: + def __init__(self, execution): self.execution = execution self.override = None @@ -365,7 +362,7 @@ def update(self, data: RobotUpdateData): else: dx = self.last_position[0] - data.position[0] dy = self.last_position[1] - data.position[1] - dist = math.sqrt(dx*dx + dy*dy) + dist = math.sqrt(dx * dx + dy * dy) if dist > 0.1: print('about to replace override schedule') self.override = self.execution.override_schedule( @@ -391,18 +388,11 @@ def update_robot(robot: RobotAdapter): if data is None: return - state = rmf_easy.RobotState( - data.map, - data.position, - data.battery_soc - ) + state = rmf_easy.RobotState(data.map, data.position, data.battery_soc) if robot.update_handle is None: robot.update_handle = robot.fleet_handle.add_robot( - robot.name, - state, - robot.configuration, - robot.make_callbacks() + robot.name, state, robot.configuration, robot.make_callbacks() ) return @@ -416,13 +406,11 @@ def ros_connections(node, robots, fleet_handle): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL + durability=Durability.TRANSIENT_LOCAL, ) closed_lanes_pub = node.create_publisher( - ClosedLanes, - 'closed_lanes', - qos_profile=transient_qos + ClosedLanes, 'closed_lanes', qos_profile=transient_qos ) closed_lanes = set() @@ -463,14 +451,14 @@ def mode_request_cb(msg): LaneRequest, 'lane_closure_requests', lane_request_cb, - qos_profile=qos_profile_system_default + qos_profile=qos_profile_system_default, ) node.create_subscription( ModeRequest, 'action_execution_notice', mode_request_cb, - qos_profile=qos_profile_system_default + qos_profile=qos_profile_system_default, ) diff --git a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py index 6b7c8dbb..4eaf6fc0 100644 --- a/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py +++ b/rmf_demos_fleet_adapter/rmf_demos_fleet_adapter/fleet_manager.py @@ -14,41 +14,38 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import math -import yaml +import argparse +import copy import json +import math +import sys +import threading import time -import copy -import argparse +from typing import Optional +from fastapi import FastAPI +import numpy as np +from pydantic import BaseModel +from pyproj import Transformer import rclpy from rclpy.node import Node from rclpy.qos import qos_profile_system_default - -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_fleet_msgs.msg import RobotState, Location, PathRequest, \ - DockSummary, RobotMode - import rmf_adapter as adpt -import rmf_adapter.vehicletraits as traits import rmf_adapter.geometry as geometry - -import numpy as np -from pyproj import Transformer - +import rmf_adapter.vehicletraits as traits +from rmf_fleet_msgs.msg import DockSummary +from rmf_fleet_msgs.msg import Location +from rmf_fleet_msgs.msg import PathRequest +from rmf_fleet_msgs.msg import RobotMode +from rmf_fleet_msgs.msg import RobotState import socketio - -from fastapi import FastAPI import uvicorn -from typing import Optional -from pydantic import BaseModel +import yaml -import threading app = FastAPI() @@ -72,6 +69,7 @@ class Response(BaseModel): # Fleet Manager # ------------------------------------------------------------------------------ class State: + def __init__(self, state: RobotState = None, destination: Location = None): self.state = state self.destination = destination @@ -82,8 +80,9 @@ def __init__(self, state: RobotState = None, destination: Location = None): self.gps_pos = [0, 0] def gps_to_xy(self, gps_json: dict): - svy21_xy = \ - self.svy_transformer.transform(gps_json['lat'], gps_json['lon']) + svy21_xy = self.svy_transformer.transform( + gps_json['lat'], gps_json['lon'] + ) self.gps_pos[0] = svy21_xy[1] self.gps_pos[1] = svy21_xy[0] @@ -95,6 +94,7 @@ def is_expected_task_id(self, task_id): class FleetManager(Node): + def __init__(self, config, nav_path): self.debug = False self.config = config @@ -117,33 +117,41 @@ def __init__(self, config, nav_path): for robot_name, _ in self.config['rmf_fleet']['robots'].items(): self.robots[robot_name] = State() - assert(len(self.robots) > 0) + assert len(self.robots) > 0 - profile = traits.Profile(geometry.make_final_convex_circle( - self.config['rmf_fleet']['profile']['footprint']), + profile = traits.Profile( + geometry.make_final_convex_circle( + self.config['rmf_fleet']['profile']['footprint'] + ), geometry.make_final_convex_circle( - self.config['rmf_fleet']['profile']['vicinity'])) + self.config['rmf_fleet']['profile']['vicinity'] + ), + ) self.vehicle_traits = traits.VehicleTraits( linear=traits.Limits( - *self.config['rmf_fleet']['limits']['linear']), + *self.config['rmf_fleet']['limits']['linear'] + ), angular=traits.Limits( - *self.config['rmf_fleet']['limits']['angular']), - profile=profile) - self.vehicle_traits.differential.reversible =\ - self.config['rmf_fleet']['reversible'] + *self.config['rmf_fleet']['limits']['angular'] + ), + profile=profile, + ) + self.vehicle_traits.differential.reversible = self.config['rmf_fleet'][ + 'reversible' + ] fleet_manager_config = self.config['fleet_manager'] self.action_paths = fleet_manager_config.get('action_paths', {}) self.sio = socketio.Client() - @self.sio.on("/gps") + @self.sio.on('/gps') def message(data): try: robot = json.loads(data) robot_name = robot['robot_id'] self.robots[robot_name].gps_to_xy(robot) except KeyError as e: - self.get_logger().info(f"Malformed GPS Message!: {e}") + self.get_logger().info(f'Malformed GPS Message!: {e}') if self.gps: while True: @@ -152,42 +160,38 @@ def message(data): break except Exception: self.get_logger().info( - f"Trying to connect to sio server at" - f"http://0.0.0.0:8080..") + 'Trying to connect to sio server at ' + 'http://0.0.0.0:8080..' + ) time.sleep(1) self.create_subscription( - RobotState, - 'robot_state', - self.robot_state_cb, - 100 + RobotState, 'robot_state', self.robot_state_cb, 100 ) transient_qos = QoSProfile( history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.create_subscription( DockSummary, 'dock_summary', self.dock_summary_cb, - qos_profile=transient_qos) + qos_profile=transient_qos, + ) self.path_pub = self.create_publisher( PathRequest, 'robot_path_requests', - qos_profile=qos_profile_system_default) + qos_profile=qos_profile_system_default, + ) - @app.get('/open-rmf/rmf_demos_fm/status/', - response_model=Response) + @app.get('/open-rmf/rmf_demos_fm/status/', response_model=Response) async def status(robot_name: Optional[str] = None): - response = { - 'data': {}, - 'success': False, - 'msg': '' - } + response = {'data': {}, 'success': False, 'msg': ''} if robot_name is None: response['data']['all_robots'] = [] for robot_name in self.robots: @@ -195,7 +199,8 @@ async def status(robot_name: Optional[str] = None): if state is None or state.state is None: return response response['data']['all_robots'].append( - self.get_robot_state(state, robot_name)) + self.get_robot_state(state, robot_name) + ) else: state = self.robots.get(robot_name) if state is None or state.state is None: @@ -204,11 +209,10 @@ async def status(robot_name: Optional[str] = None): response['success'] = True return response - @app.post('/open-rmf/rmf_demos_fm/navigate/', - response_model=Response) + @app.post('/open-rmf/rmf_demos_fm/navigate/', response_model=Response) async def navigate(robot_name: str, cmd_id: int, dest: Request): response = {'success': False, 'msg': ''} - if (robot_name not in self.robots or len(dest.destination) < 1): + if robot_name not in self.robots or len(dest.destination) < 1: return response robot = self.robots[robot_name] @@ -233,9 +237,12 @@ async def navigate(robot_name: str, cmd_id: int, dest: Request): path_request.path.append(cur_loc) disp = self.disp([target_x, target_y], [cur_x, cur_y]) - duration = int(disp/self.vehicle_traits.linear.nominal_velocity) +\ - int(abs(abs(cur_yaw) - abs(target_yaw)) / - self.vehicle_traits.rotational.nominal_velocity) + duration = int( + disp / self.vehicle_traits.linear.nominal_velocity + ) + int( + abs(abs(cur_yaw) - abs(target_yaw)) + / self.vehicle_traits.rotational.nominal_velocity + ) t.sec = t.sec + duration target_loc = Location() target_loc.t = t @@ -262,8 +269,7 @@ async def navigate(robot_name: str, cmd_id: int, dest: Request): response['success'] = True return response - @app.get('/open-rmf/rmf_demos_fm/stop_robot/', - response_model=Response) + @app.get('/open-rmf/rmf_demos_fm/stop_robot/', response_model=Response) async def stop(robot_name: str, cmd_id: int): response = {'success': False, 'msg': ''} if robot_name not in self.robots: @@ -290,8 +296,9 @@ async def stop(robot_name: str, cmd_id: int): response['success'] = True return response - @app.get('/open-rmf/rmf_demos_fm/action_paths/', - response_model=Response) + @app.get( + '/open-rmf/rmf_demos_fm/action_paths/', response_model=Response + ) async def action_paths(activity: str, label: str): response = {'success': False, 'msg': ''} if activity not in self.action_paths: @@ -304,12 +311,11 @@ async def action_paths(activity: str, label: str): response['success'] = True return response - @app.post('/open-rmf/rmf_demos_fm/start_activity/', - response_model=Response) + @app.post( + '/open-rmf/rmf_demos_fm/start_activity/', response_model=Response + ) async def start_activity( - robot_name: str, - cmd_id: int, - request: Request + robot_name: str, cmd_id: int, request: Request ): response = {'success': False, 'msg': ''} if ( @@ -354,11 +360,12 @@ async def start_activity( response['data']['path'] = activity_path return response - @app.post('/open-rmf/rmf_demos_fm/toggle_teleop/', - response_model=Response) + @app.post( + '/open-rmf/rmf_demos_fm/toggle_teleop/', response_model=Response + ) async def toggle_teleop(robot_name: str, mode: Request): response = {'success': False, 'msg': ''} - if (robot_name not in self.robots): + if robot_name not in self.robots: return response # Toggle action mode self.robots[robot_name].mode_teleop = mode.toggle @@ -366,10 +373,12 @@ async def toggle_teleop(robot_name: str, mode: Request): return response def robot_state_cb(self, msg): - if (msg.name in self.robots): + if msg.name in self.robots: robot = self.robots[msg.name] - if not robot.is_expected_task_id(msg.task_id) and \ - not robot.mode_teleop: + if ( + not robot.is_expected_task_id(msg.task_id) + and not robot.mode_teleop + ): # This message is out of date, so disregard it. if robot.last_path_request is not None: # Resend the latest task request for this robot, in case @@ -389,12 +398,9 @@ def robot_state_cb(self, msg): return if ( - ( - msg.mode.mode == RobotMode.MODE_IDLE - or msg.mode.mode == RobotMode.MODE_CHARGING - ) - and len(msg.path) == 0 - ): + msg.mode.mode == RobotMode.MODE_IDLE + or msg.mode.mode == RobotMode.MODE_CHARGING + ) and len(msg.path) == 0: robot = self.robots[msg.name] robot.destination = None completed_request = int(msg.task_id) @@ -408,7 +414,7 @@ def robot_state_cb(self, msg): def dock_summary_cb(self, msg): for fleet in msg.docks: - if (fleet.fleet_name == self.fleet_name): + if fleet.fleet_name == self.fleet_name: for dock in fleet.params: self.docks[dock.start] = dock.path @@ -421,32 +427,34 @@ def get_robot_state(self, robot: State, robot_name): angle = robot.state.location.yaw data['robot_name'] = robot_name data['map_name'] = robot.state.location.level_name - data['position'] =\ - {'x': position[0], 'y': position[1], 'yaw': angle} + data['position'] = {'x': position[0], 'y': position[1], 'yaw': angle} data['battery'] = robot.state.battery_percent - if (robot.destination is not None - and robot.last_path_request is not None): + if ( + robot.destination is not None + and robot.last_path_request is not None + ): destination = robot.destination # remove offset for calculation if using gps coords if self.gps: position[0] -= self.offset[0] position[1] -= self.offset[1] # calculate arrival estimate - dist_to_target =\ - self.disp(position, [destination.x, destination.y]) + dist_to_target = self.disp( + position, [destination.x, destination.y] + ) ori_delta = abs(abs(angle) - abs(destination.yaw)) if ori_delta > np.pi: ori_delta = ori_delta - (2 * np.pi) if ori_delta < -np.pi: ori_delta = (2 * np.pi) + ori_delta - duration = (dist_to_target / - self.vehicle_traits.linear.nominal_velocity + - ori_delta / - self.vehicle_traits.rotational.nominal_velocity) + duration = ( + dist_to_target / self.vehicle_traits.linear.nominal_velocity + + ori_delta / self.vehicle_traits.rotational.nominal_velocity + ) cmd_id = int(robot.last_path_request.task_id) data['destination_arrival'] = { 'cmd_id': cmd_id, - 'duration': duration + 'duration': duration, } else: data['destination_arrival'] = None @@ -471,7 +479,7 @@ def get_robot_state(self, robot: State, robot_name): return data def disp(self, A, B): - return math.sqrt((A[0]-B[0])**2 + (A[1]-B[1])**2) + return math.sqrt((A[0] - B[0]) ** 2 + (A[1] - B[1]) ** 2) # ------------------------------------------------------------------------------ @@ -484,16 +492,27 @@ def main(argv=sys.argv): args_without_ros = rclpy.utilities.remove_ros_args(argv) parser = argparse.ArgumentParser( - prog="fleet_adapter", - description="Configure and spin up the fleet adapter") - parser.add_argument("-c", "--config_file", type=str, required=True, - help="Path to the config.yaml file") - parser.add_argument("-n", "--nav_graph", type=str, required=True, - help="Path to the nav_graph for this fleet adapter") + prog='fleet_adapter', + description='Configure and spin up the fleet adapter', + ) + parser.add_argument( + '-c', + '--config_file', + type=str, + required=True, + help='Path to the config.yaml file', + ) + parser.add_argument( + '-n', + '--nav_graph', + type=str, + required=True, + help='Path to the nav_graph for this fleet adapter', + ) args = parser.parse_args(args_without_ros[1:]) - print(f"Starting fleet manager...") + print('Starting fleet manager...') - with open(args.config_file, "r") as f: + with open(args.config_file, 'r') as f: config = yaml.safe_load(f) fleet_manager = FleetManager(config, args.nav_graph) @@ -505,7 +524,7 @@ def main(argv=sys.argv): app, host=config['fleet_manager']['ip'], port=config['fleet_manager']['port'], - log_level='warning' + log_level='warning', ) diff --git a/rmf_demos_fleet_adapter/setup.py b/rmf_demos_fleet_adapter/setup.py index e352424b..0efd45c9 100644 --- a/rmf_demos_fleet_adapter/setup.py +++ b/rmf_demos_fleet_adapter/setup.py @@ -1,6 +1,8 @@ -import os from glob import glob -from setuptools import setup, find_packages +import os + +from setuptools import find_packages +from setuptools import setup package_name = 'rmf_demos_fleet_adapter' @@ -9,19 +11,23 @@ version='2.1.2', packages=find_packages(), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ( + 'share/ament_index/resource_index/packages', + ['resource/' + package_name], + ), ('share/' + package_name, ['package.xml']), ('share/' + package_name, ['config.yaml']), - (os.path.join('share', package_name, 'launch'), - glob('launch/*.launch.xml')), + ( + os.path.join('share', package_name, 'launch'), + glob('launch/*.launch.xml'), + ), ], install_requires=['setuptools', 'fastapi>=0.79.0', 'uvicorn>=0.18.2'], zip_safe=True, maintainer='Xi Yu Oh', maintainer_email='xiyu@openrobotics.org', description='Fleet adapters for interfacing with RMF Demos robots with a ' - 'fleet manager via REST API', + 'fleet manager via REST API', license='Apache License 2.0', tests_require=['pytest'], entry_points={ diff --git a/rmf_demos_fleet_adapter/test/test_flake8.py b/rmf_demos_fleet_adapter/test/test_flake8.py index 27ee1078..26030113 100644 --- a/rmf_demos_fleet_adapter/test/test_flake8.py +++ b/rmf_demos_fleet_adapter/test/test_flake8.py @@ -20,6 +20,6 @@ @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) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len( + errors + ) + '\n'.join(errors) diff --git a/rmf_demos_panel/rmf_demos_panel/dispatcher_client.py b/rmf_demos_panel/rmf_demos_panel/dispatcher_client.py index 55b6b367..5a9e3cdb 100644 --- a/rmf_demos_panel/rmf_demos_panel/dispatcher_client.py +++ b/rmf_demos_panel/rmf_demos_panel/dispatcher_client.py @@ -1,4 +1,3 @@ - # Copyright 2021 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,26 +12,27 @@ # See the License for the specific language governing permissions and # limitations under the License. -import rclpy -import time import json -import uuid +import time from typing import Tuple +import uuid +import rclpy from rclpy.node import Node -from rclpy.time import Time from rclpy.parameter import Parameter # Qos from rclpy.qos import qos_profile_system_default -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_task_msgs.msg import ApiRequest, ApiResponse +from rclpy.time import Time from rmf_building_map_msgs.srv import GetBuildingMap -from rmf_fleet_msgs.msg import FleetState, RobotMode +from rmf_fleet_msgs.msg import FleetState +from rmf_fleet_msgs.msg import RobotMode +from rmf_task_msgs.msg import ApiRequest +from rmf_task_msgs.msg import ApiResponse ############################################################################### @@ -44,17 +44,23 @@ def __init__(self): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.task_api_req_pub = self.create_publisher( - ApiRequest, '/task_api_requests', api_req_qos_profile) + ApiRequest, '/task_api_requests', api_req_qos_profile + ) self.get_building_map_srv = self.create_client( - GetBuildingMap, '/get_building_map') + GetBuildingMap, '/get_building_map' + ) # to show robot states self.fleet_state_subscription = self.create_subscription( - FleetState, 'fleet_states', self.fleet_state_cb, - qos_profile=QoSProfile(depth=20)) + FleetState, + 'fleet_states', + self.fleet_state_cb, + qos_profile=QoSProfile(depth=20), + ) self.fleet_states_dict = {} # TODO remove this @@ -86,35 +92,32 @@ def submit_task_request(self, req_json) -> Tuple[str, str]: request_json, err_msg = self.__convert_task_description(req_json) if request_json is None: self.get_logger().error(err_msg) - return "", err_msg - payload = { - "type": "dispatch_task_request", - "request": request_json - } + return '', err_msg + payload = {'type': 'dispatch_task_request', 'request': request_json} msg = ApiRequest() - msg.request_id = "demos_" + str(uuid.uuid4()) + msg.request_id = 'demos_' + str(uuid.uuid4()) msg.json_msg = json.dumps(payload) self.task_api_req_pub.publish(msg) self.get_logger().info(f'Publish task request {msg}') # Note: API Response or can wait for response # TODO: listen to "/task_api_responses" - return msg.request_id, "" # success + return msg.request_id, '' # success def cancel_task_request(self, task_id) -> bool: """ Cancel Task - This function will trigger a ros srv call to the dispatcher node, and return a response. """ - print(f"Canceling Task Request! {task_id}") + print(f'Canceling Task Request! {task_id}') payload = { - "type": "cancel_task_request", - "task_id": task_id, - "labels": ["cancellation from simple api server"] + 'type': 'cancel_task_request', + 'task_id': task_id, + 'labels': ['cancellation from simple api server'], } msg = ApiRequest() - msg.request_id = "demos_" + str(uuid.uuid4()) + msg.request_id = 'demos_' + str(uuid.uuid4()) msg.json_msg = json.dumps(payload) # self.task_api_req_pub.publish(msg) @@ -155,11 +158,13 @@ def get_building_map_data(self): else: # Return building map map_data = self.__convert_building_map_msg( - response.building_map) + response.building_map + ) return map_data except Exception as e: self.get_logger().error( - 'Error! GetBuildingMap Srv failed %r' % (e,)) + 'Error! GetBuildingMap Srv failed %r' % (e,) + ) return {} # empty dict def set_task_state(self, json_obj): @@ -167,74 +172,85 @@ def set_task_state(self, json_obj): set and store the latest task_state. """ state = self.__convert_task_state_msg(json_obj) - id = state["task_id"] + id = state['task_id'] self.task_states_cache[id] = state -############################################################################### + ########################################################################### def __convert_task_state_msg(self, json_obj): """ convert task_state v2 msg to legacy dashbaord json msg format """ task_state = {} - task_state["task_id"] = json_obj["booking"]["id"] - task_state["state"] = json_obj["status"] - task_state["fleet_name"] = json_obj["assigned_to"]["group"] - task_state["robot_name"] = json_obj["assigned_to"]["name"] - task_state["task_type"] = json_obj["category"] - task_state["priority"] = 0 # TODO + task_state['task_id'] = json_obj['booking']['id'] + task_state['state'] = json_obj['status'] + task_state['fleet_name'] = json_obj['assigned_to']['group'] + task_state['robot_name'] = json_obj['assigned_to']['name'] + task_state['task_type'] = json_obj['category'] + task_state['priority'] = 0 # TODO # Note: all in seconds - task_state["start_time"] = round( - json_obj["unix_millis_start_time"]/1000.0, 2) - task_state["end_time"] = round( - json_obj["unix_millis_finish_time"]/1000.0, 2) - task_state["submited_start_time"] = round( - json_obj["booking"]["unix_millis_earliest_start_time"]/1000.0, 2) + task_state['start_time'] = round( + json_obj['unix_millis_start_time'] / 1000.0, 2 + ) + task_state['end_time'] = round( + json_obj['unix_millis_finish_time'] / 1000.0, 2 + ) + task_state['submited_start_time'] = round( + json_obj['booking']['unix_millis_earliest_start_time'] / 1000.0, 2 + ) # \note description should be: e.g. cleaning zone - if "active" in json_obj: - active_phase = json_obj["active"] - task_state["description"] = json_obj["phases"][str( - active_phase)]["detail"] + if 'active' in json_obj: + active_phase = json_obj['active'] + task_state['description'] = json_obj['phases'][str(active_phase)][ + 'detail' + ] else: - task_state["description"] = "queued" + task_state['description'] = 'queued' # use start_time, end_time and current time to compute percent - predicted_duration = task_state["end_time"] - task_state["start_time"] - task_duration = self.ros_time() - task_state["start_time"] - percent = task_duration/predicted_duration - if (percent > 1.0): - task_state["progress"] = f"100%" - elif (task_state["state"] == "completed"): - task_state["progress"] = f"100%" - elif(percent < 0.0): - task_state["progress"] = f"0%" + predicted_duration = task_state['end_time'] - task_state['start_time'] + task_duration = self.ros_time() - task_state['start_time'] + percent = task_duration / predicted_duration + if percent > 1.0: + task_state['progress'] = f'100%' + elif task_state['state'] == 'completed': + task_state['progress'] = f'100%' + elif percent < 0.0: + task_state['progress'] = f'0%' else: - percent = int(100.0*percent) - task_state["progress"] = f"{percent}%" + percent = int(100.0 * percent) + task_state['progress'] = f'{percent}%' done_status_type = [ - "uninitialized", "blocked", "error", "failed", - "skipped", "canceled", "killed", "completed"] - if task_state["state"] in done_status_type: - task_state["done"] = True + 'uninitialized', + 'blocked', + 'error', + 'failed', + 'skipped', + 'canceled', + 'killed', + 'completed', + ] + if task_state['state'] in done_status_type: + task_state['done'] = True else: - task_state["done"] = False + task_state['done'] = False # Hack, change to capital letter for frontend compliance - task_state["state"] = task_state["state"].title() + task_state['state'] = task_state['state'].title() return task_state def __get_robot_assignment(self, robot_name): assigned_tasks = [] assigned_task_ids = [] for _, state in self.task_states_cache.items(): - if state["robot_name"] == robot_name: + if state['robot_name'] == robot_name: assigned_tasks.append(state) assigned_tasks.sort(key=lambda x: x.get('start_time')) for task in assigned_tasks: - assigned_task_ids.append(task["task_id"]) + assigned_task_ids.append(task['task_id']) return assigned_task_ids def __convert_robot_states_msg(self, fleet_name, robot_states): @@ -243,27 +259,27 @@ def __convert_robot_states_msg(self, fleet_name, robot_states): """ bots = [] mode_enum = { - RobotMode.MODE_IDLE: "Idle-0", - RobotMode.MODE_CHARGING: "Charging-1", - RobotMode.MODE_MOVING: "Moving-2", - RobotMode.MODE_PAUSED: "Paused-3", - RobotMode.MODE_WAITING: "Waiting-4", - RobotMode.MODE_EMERGENCY: "Emengency-5", - RobotMode.MODE_GOING_HOME: "GoingHome-6", - RobotMode.MODE_DOCKING: "Dock/Clean-7", - RobotMode.MODE_ADAPTER_ERROR: "AdpterError-8" + RobotMode.MODE_IDLE: 'Idle-0', + RobotMode.MODE_CHARGING: 'Charging-1', + RobotMode.MODE_MOVING: 'Moving-2', + RobotMode.MODE_PAUSED: 'Paused-3', + RobotMode.MODE_WAITING: 'Waiting-4', + RobotMode.MODE_EMERGENCY: 'Emengency-5', + RobotMode.MODE_GOING_HOME: 'GoingHome-6', + RobotMode.MODE_DOCKING: 'Dock/Clean-7', + RobotMode.MODE_ADAPTER_ERROR: 'AdpterError-8', } for bot in robot_states: state = {} - state["robot_name"] = bot.name - state["fleet_name"] = fleet_name - state["mode"] = mode_enum[bot.mode.mode] - state["battery_percent"] = bot.battery_percent - state["location_x"] = bot.location.x - state["location_y"] = bot.location.y - state["location_yaw"] = bot.location.yaw - state["level_name"] = bot.location.level_name - state["assignments"] = self.__get_robot_assignment(bot.name) + state['robot_name'] = bot.name + state['fleet_name'] = fleet_name + state['mode'] = mode_enum[bot.mode.mode] + state['battery_percent'] = bot.battery_percent + state['location_x'] = bot.location.x + state['location_y'] = bot.location.y + state['location_yaw'] = bot.location.yaw + state['level_name'] = bot.location.level_name + state['assignments'] = self.__get_robot_assignment(bot.name) bots.append(state) return bots @@ -274,131 +290,139 @@ def __convert_task_description(self, task_json): """ # default request fields request = { - "priority": {"type": "binary", "value": 0}, - "labels": ["rmf_demos.simple_api_server"], - "description": {} + 'priority': {'type': 'binary', 'value': 0}, + 'labels': ['rmf_demos.simple_api_server'], + 'description': {}, } try: - if (("task_type" not in task_json) or - ("start_time" not in task_json) or - ("description" not in task_json)): - raise Exception("Key value is incomplete") - - if ("priority" in task_json): - priority_val = int(task_json["priority"]) - if (priority_val < 0): - raise Exception("Priority value is less than 0") - request["priority"]["value"] = priority_val + if ( + ('task_type' not in task_json) + or ('start_time' not in task_json) + or ('description' not in task_json) + ): + raise Exception('Key value is incomplete') + + if 'priority' in task_json: + priority_val = int(task_json['priority']) + if priority_val < 0: + raise Exception('Priority value is less than 0') + request['priority']['value'] = priority_val # Refer to task schemas # https://github.com/open-rmf/rmf_ros2/blob/redesign_v2/rmf_fleet_adapter/schemas - desc = task_json["description"] - if task_json["task_type"] == "Clean": - request["category"] = "clean" - request["description"]["zone"] = desc["cleaning_zone"] - elif task_json["task_type"] == "Loop": - request["category"] = "patrol" - request["description"]["places"] = [ - desc["start_name"], - desc["finish_name"]] - request["description"]["rounds"] = int(desc["num_loops"]) - elif task_json["task_type"] == "Delivery": - request["category"] = "delivery" - request["description"]["pickup"] = { - "place": desc["pickup_place_name"], - "handler": desc["pickup_dispenser"], - "payload": []} - request["description"]["dropoff"] = { - "place": desc["dropoff_place_name"], - "handler": desc["dropoff_ingestor"], - "payload": []} + desc = task_json['description'] + if task_json['task_type'] == 'Clean': + request['category'] = 'clean' + request['description']['zone'] = desc['cleaning_zone'] + elif task_json['task_type'] == 'Loop': + request['category'] = 'patrol' + request['description']['places'] = [ + desc['start_name'], + desc['finish_name'], + ] + request['description']['rounds'] = int(desc['num_loops']) + elif task_json['task_type'] == 'Delivery': + request['category'] = 'delivery' + request['description']['pickup'] = { + 'place': desc['pickup_place_name'], + 'handler': desc['pickup_dispenser'], + 'payload': [], + } + request['description']['dropoff'] = { + 'place': desc['dropoff_place_name'], + 'handler': desc['dropoff_ingestor'], + 'payload': [], + } else: - raise Exception("Invalid TaskType") + raise Exception('Invalid TaskType') # Calc earliest_start_time, convert "Duration from now(min)" # to unix_milli epoch time rclpy.spin_once(self, timeout_sec=0.0) rostime_now = self.get_clock().now() - unix_milli_time = round(rostime_now.nanoseconds/1e6) - unix_milli_time += int(task_json["start_time"]*60*1000) - request["unix_millis_earliest_start_time"] = unix_milli_time + unix_milli_time = round(rostime_now.nanoseconds / 1e6) + unix_milli_time += int(task_json['start_time'] * 60 * 1000) + request['unix_millis_earliest_start_time'] = unix_milli_time except KeyError as ex: - return None, f"Missing Key value in json body: {ex}" + return None, f'Missing Key value in json body: {ex}' except Exception as ex: return None, str(ex) - print("return", request) - return request, "" + print('return', request) + return request, '' def __convert_building_map_msg(self, msg): map_data = {} - map_data["name"] = msg.name - map_data["levels"] = [] + map_data['name'] = msg.name + map_data['levels'] = [] for level in msg.levels: level_data = {} - level_data["name"] = level.name - level_data["elevation"] = level.elevation + level_data['name'] = level.name + level_data['elevation'] = level.elevation # TODO: Images, places, doors? - level_data["nav_graphs"] = \ - [self.__convert_graph_msg(msg) for msg in level.nav_graphs] - level_data["wall_graph"] = \ - self.__convert_graph_msg(level.wall_graph) + level_data['nav_graphs'] = [ + self.__convert_graph_msg(msg) for msg in level.nav_graphs + ] + level_data['wall_graph'] = self.__convert_graph_msg( + level.wall_graph + ) - map_data["levels"].append(level_data) + map_data['levels'].append(level_data) return map_data def __convert_graph_msg(self, graph_msg): graph_data = {} - graph_data["name"] = graph_msg.name - graph_data["vertices"] = [] - graph_data["edges"] = [] + graph_data['name'] = graph_msg.name + graph_data['vertices'] = [] + graph_data['edges'] = [] for vertex in graph_msg.vertices: vertex_data = {} - vertex_data["x"] = vertex.x - vertex_data["y"] = vertex.y - vertex_data["name"] = \ - vertex.name + vertex_data['x'] = vertex.x + vertex_data['y'] = vertex.y + vertex_data['name'] = vertex.name - vertex_data["params"] = \ - [self.__convert_param_msg(msg) for msg in vertex.params] + vertex_data['params'] = [ + self.__convert_param_msg(msg) for msg in vertex.params + ] - graph_data["vertices"].append(vertex_data) + graph_data['vertices'].append(vertex_data) for edge in graph_msg.edges: edge_data = {} - edge_data["v1_idx"] = edge.v1_idx - edge_data["v2_idx"] = edge.v2_idx - edge_data["edge_type"] = edge.edge_type + edge_data['v1_idx'] = edge.v1_idx + edge_data['v2_idx'] = edge.v2_idx + edge_data['edge_type'] = edge.edge_type - edge_data["params"] = \ - [self.__convert_param_msg(msg) for msg in edge.params] + edge_data['params'] = [ + self.__convert_param_msg(msg) for msg in edge.params + ] - graph_data["edges"].append(edge_data) + graph_data['edges'].append(edge_data) return graph_data def __convert_param_msg(self, param_msg): param_data = {} - param_data["name"] = param_msg.name + param_data['name'] = param_msg.name # TODO: how to handle TYPE_UNDEFINED? if param_msg.type == param_msg.TYPE_STRING: - param_data["type"] = "string" - param_data["value"] = str(param_msg.value_string) + param_data['type'] = 'string' + param_data['value'] = str(param_msg.value_string) elif param_msg.type == param_msg.TYPE_INT: - param_data["type"] = "int" - param_data["value"] = str(param_msg.value_int) + param_data['type'] = 'int' + param_data['value'] = str(param_msg.value_int) elif param_msg.type == param_msg.TYPE_DOUBLE: - param_data["type"] = "double" - param_data["value"] = str(param_msg.value_float) + param_data['type'] = 'double' + param_data['value'] = str(param_msg.value_float) elif param_msg.type == param_msg.TYPE_BOOL: - param_data["type"] = "bool" - param_data["value"] = str(param_msg.value_bool) + param_data['type'] = 'bool' + param_data['value'] = str(param_msg.value_bool) else: - param_data["value"] = None + param_data['value'] = None return param_data diff --git a/rmf_demos_panel/rmf_demos_panel/rmf_msg_observer.py b/rmf_demos_panel/rmf_demos_panel/rmf_msg_observer.py index caa8db37..1e34b0cd 100644 --- a/rmf_demos_panel/rmf_demos_panel/rmf_msg_observer.py +++ b/rmf_demos_panel/rmf_demos_panel/rmf_msg_observer.py @@ -20,18 +20,19 @@ import asyncio -import websockets import json -from typing import Callable, Optional, List, Dict, Tuple +from typing import Callable, Dict, List, Optional, Tuple + +import websockets ##################################################################### class RmfMsgType: - FleetState = "fleet_state_update" - TaskState = "task_state_update" - TaskLog = "task_log_update" - FleetLog = "fleet_log_update" + FleetState = 'fleet_state_update' + TaskState = 'task_state_update' + TaskLog = 'task_log_update' + FleetLog = 'fleet_log_update' """ @@ -45,26 +46,27 @@ class RmfMsgType: def filter_rmf_msg( - json_str: str, - filters: Dict[RmfMsgType, List[str]] = {} + json_str: str, filters: Dict[RmfMsgType, List[str]] = {} ) -> Optional[Tuple[RmfMsgType, Dict]]: obj = json.loads(json_str) - if "type" not in obj: - print("ERRORRRR: type is not avail as json key") + if 'type' not in obj: + print('ERRORRRR: type is not avail as json key') return None - if obj["type"] not in filters: + if obj['type'] not in filters: return None - msg_type = obj["type"] - data = obj["data"] + msg_type = obj['type'] + data = obj['data'] if not filters[msg_type]: # empty list return msg_type, data for filter in filters[msg_type]: if filter not in data: - print(f" Key ERROR!!, indicated data_filter: " - "[{filter}] is not avail in {data}") + print( + f' Key ERROR!!, indicated data_filter: ' + '[{filter}] is not avail in {data}' + ) return None data = data[filter] @@ -85,13 +87,14 @@ class AsyncRmfMsgObserver: :param data_filter: detailed filter different levels of the data obj """ - def __init__(self, - callback_fn: Callable[[dict], None], - server_url: str = "localhost", - server_port: str = "7878", - msg_filters: Dict[RmfMsgType, List[str]] = {} - ): - print("Starting Websocket Server") + def __init__( + self, + callback_fn: Callable[[dict], None], + server_url: str = 'localhost', + server_port: str = '7878', + msg_filters: Dict[RmfMsgType, List[str]] = {}, + ): + print('Starting Websocket Server') self.callback_fn = callback_fn self.server_url = server_url self.server_port = server_port @@ -110,8 +113,7 @@ def spin(self, future=asyncio.Future()): async def __msg_handler(self, websocket, path): try: async for message in websocket: - ret_data = filter_rmf_msg( - message, self.msg_filters) + ret_data = filter_rmf_msg(message, self.msg_filters) if ret_data: # call the provided callback function msg_type, data = ret_data @@ -122,11 +124,12 @@ async def __msg_handler(self, websocket, path): async def __check_future(self): while not self.future.done(): await asyncio.sleep(1) # arbitrary loop freq check - print("Received exit signal") + print('Received exit signal') # TODO(YL): debug the reason why future is not awaitable outside # of the loop problem. Thweb_server_spinap_future(self.future) async def __internal_spin(self): async with websockets.serve( - self.__msg_handler, self.server_url, self.server_port): + self.__msg_handler, self.server_url, self.server_port + ): await self.__check_future() diff --git a/rmf_demos_panel/rmf_demos_panel/simple_api_server.py b/rmf_demos_panel/rmf_demos_panel/simple_api_server.py index 434bfd4e..7017c3f0 100644 --- a/rmf_demos_panel/rmf_demos_panel/simple_api_server.py +++ b/rmf_demos_panel/rmf_demos_panel/simple_api_server.py @@ -19,47 +19,55 @@ 2) socketIO broadcast states: /task_status, /robot_states, /ros_time """ -import sys -import os -import rclpy import argparse -import time +import asyncio import json import logging +import os +import sys from threading import Thread +import time -from flask import Flask, request, jsonify +from flask import Flask +from flask import jsonify +from flask import request from flask_cors import CORS -from flask_socketio import SocketIO, emit, disconnect -import asyncio +from flask_socketio import disconnect +from flask_socketio import emit +from flask_socketio import SocketIO +import rclpy from rmf_demos_panel.dispatcher_client import DispatcherClient -from rmf_demos_panel.rmf_msg_observer import AsyncRmfMsgObserver, RmfMsgType +from rmf_demos_panel.rmf_msg_observer import AsyncRmfMsgObserver +from rmf_demos_panel.rmf_msg_observer import RmfMsgType ############################################################################### app = Flask(__name__) -CORS(app, origins=r"/*") +CORS(app, origins=r'/*') socketio = SocketIO(app, async_mode='threading') -socketio.init_app(app, cors_allowed_origins="*") +socketio.init_app(app, cors_allowed_origins='*') rclpy.init(args=None) dispatcher_client = DispatcherClient() # logging config logging.getLogger('werkzeug').setLevel(logging.ERROR) # hide logs from flask -logging.basicConfig(level=logging.DEBUG, - format='%(asctime)s %(levelname)s %(message)s', - filename='web_server.log', - filemode='w') +logging.basicConfig( + level=logging.DEBUG, + format='%(asctime)s %(levelname)s %(message)s', + filename='web_server.log', + filemode='w', +) # default dashboard -dashboard_config = {"world_name": "EMPTY_DASHBOARD_CONFIG", - "valid_task": [], - "task": {"Delivery": {}, "Loop": {}, "Clean": {}} - } +dashboard_config = { + 'world_name': 'EMPTY_DASHBOARD_CONFIG', + 'valid_task': [], + 'task': {'Delivery': {}, 'Loop': {}, 'Clean': {}}, +} ############################################################################### @@ -68,50 +76,61 @@ def submit(): """REST Call to submit task""" task_id, err_msg = dispatcher_client.submit_task_request(request.json) - logging.debug(f" ROS Time: {dispatcher_client.ros_time()} | \ - Task Submission: {json.dumps(request.json)}, error: {err_msg}") - return jsonify({"task_id": task_id, "error_msg": err_msg}) + logging.debug( + f' ROS Time: {dispatcher_client.ros_time()} | \ + Task Submission: {json.dumps(request.json)}, error: {err_msg}' + ) + return jsonify({'task_id': task_id, 'error_msg': err_msg}) @app.route('/cancel_task', methods=['POST']) def cancel(): cancel_id = request.json['task_id'] cancel_success = dispatcher_client.cancel_task_request(cancel_id) - logging.debug(f" ROS Time: {dispatcher_client.ros_time()} | \ - Cancel Task: {cancel_id}, success: {cancel_success}") - return jsonify({"success": cancel_success}) + logging.debug( + f' ROS Time: {dispatcher_client.ros_time()} | \ + Cancel Task: {cancel_id}, success: {cancel_success}' + ) + return jsonify({'success': cancel_success}) @app.route('/task_list', methods=['GET']) def status(): task_status = jsonify(dispatcher_client.get_task_status()) - logging.debug(f" ROS Time: {dispatcher_client.ros_time()} | \ - Task Status: {json.dumps(task_status.json)}") + logging.debug( + f' ROS Time: {dispatcher_client.ros_time()} | \ + Task Status: {json.dumps(task_status.json)}' + ) return task_status @app.route('/robot_list', methods=['GET']) def robots(): robot_status = jsonify(dispatcher_client.get_robot_states()) - logging.debug(f" ROS Time: {dispatcher_client.ros_time()} | \ - Robot Status: {robot_status}") + logging.debug( + f' ROS Time: {dispatcher_client.ros_time()} | \ + Robot Status: {robot_status}' + ) return robot_status @app.route('/building_map', methods=['GET']) def building_map(): building_map_data = jsonify(dispatcher_client.get_building_map_data()) - logging.debug(f" ROS Time: {dispatcher_client.ros_time()} | \ - building_map_data: {building_map_data}") + logging.debug( + f' ROS Time: {dispatcher_client.ros_time()} | \ + building_map_data: {building_map_data}' + ) return building_map_data # Note: Get Dashboard Config for each "World", specific to rmf_demos impl -@app.route("/dashboard_config", methods=['GET']) +@app.route('/dashboard_config', methods=['GET']) def config(): config = jsonify(dashboard_config) return config + ############################################################################### @@ -135,10 +154,12 @@ def broadcast_states(): socketio.emit('task_status', tasks, broadcast=True, namespace=ns) socketio.emit('robot_states', robots, broadcast=True, namespace=ns) socketio.emit('ros_time', ros_time, broadcast=True, namespace=ns) - logging.debug(f" ROS Time: {ros_time} | " - " tasks: " - f"{len(dispatcher_client.task_states_cache)}" - f" | active robots: {len(robots)}") + logging.debug( + f' ROS Time: {ros_time} | ' + ' tasks: ' + f'{len(dispatcher_client.task_states_cache)}' + f' | active robots: {len(robots)}' + ) time.sleep(2) @@ -152,48 +173,49 @@ def msg_callback(msg_type, data): observer = AsyncRmfMsgObserver( msg_callback, msg_filters={RmfMsgType.TaskState: []}, - server_url="localhost", - server_port=int(port_num) + server_url='localhost', + server_port=int(port_num), ) observer.spin(done_fut) + ############################################################################### def main(args=None): - server_ip = "0.0.0.0" + server_ip = '0.0.0.0' port_num = 8083 ws_port_num = 7878 - if "RMF_DEMOS_API_SERVER_IP" in os.environ: + if 'RMF_DEMOS_API_SERVER_IP' in os.environ: server_ip = os.environ['RMF_DEMOS_API_SERVER_IP'] - print(f"Set Server IP to: {server_ip}") + print(f'Set Server IP to: {server_ip}') - if "RMF_DEMOS_API_SERVER_PORT" in os.environ: + if 'RMF_DEMOS_API_SERVER_PORT' in os.environ: port_num = int(os.environ['RMF_DEMOS_API_SERVER_PORT']) - print(f"Set Server port to: {server_ip}:{port_num}") + print(f'Set Server port to: {server_ip}:{port_num}') - if "RMF_WS_SERVER_PORT" in os.environ: + if 'RMF_WS_SERVER_PORT' in os.environ: ws_port_num = int(os.environ['RMF_WS_SERVER_PORT']) - print(f"Set RMF Websocket port to: localhost:{ws_port_num}") + print(f'Set RMF Websocket port to: localhost:{ws_port_num}') - if "DASHBOARD_CONFIG_PATH" in os.environ: + if 'DASHBOARD_CONFIG_PATH' in os.environ: config_path = os.environ['DASHBOARD_CONFIG_PATH'] if not config_path: - print(f"WARN! env DASHBOARD_CONFIG_PATH is empty...") + print(f'WARN! env DASHBOARD_CONFIG_PATH is empty...') elif not os.path.exists(config_path): - raise FileNotFoundError(f"\n File [{config_path}] doesnt exist") + raise FileNotFoundError(f'\n File [{config_path}] doesnt exist') else: try: f = open(config_path, 'r') global dashboard_config dashboard_config = json.load(f) except Exception as err: - print(f"Failed to read [{config_path}] dashboard config file") + print(f'Failed to read [{config_path}] dashboard config file') raise err else: - print(f"WARN! env DASHBOARD_CONFIG_PATH is not specified...") + print(f'WARN! env DASHBOARD_CONFIG_PATH is not specified...') spin_thread = Thread(target=web_server_spin, args=()) spin_thread.start() @@ -203,17 +225,20 @@ def main(args=None): done_fut = asyncio.Future() listener_thread = Thread( - target=rmf_state_listener, args=(ws_port_num, done_fut)) + target=rmf_state_listener, args=(ws_port_num, done_fut) + ) listener_thread.start() - print(f"Starting RMF_Demos API Server: {server_ip}:{port_num}, " - f"with ws://localhost:{ws_port_num}") + print( + f'Starting RMF_Demos API Server: {server_ip}:{port_num}, ' + f'with ws://localhost:{ws_port_num}' + ) app.run(host=server_ip, port=port_num, debug=False) dispatcher_client.destroy_node() rclpy.shutdown() - print("shutting down...") + print('shutting down...') done_fut.set_result(True) # shutdown listner -if __name__ == "__main__": +if __name__ == '__main__': main(sys.argv) diff --git a/rmf_demos_panel/setup.py b/rmf_demos_panel/setup.py index 394e719d..2a5a3427 100644 --- a/rmf_demos_panel/setup.py +++ b/rmf_demos_panel/setup.py @@ -1,9 +1,10 @@ -from setuptools import setup from glob import glob import sys +from setuptools import setup + package_name = 'rmf_demos_panel' -py_version = ".".join(map(str, sys.version_info[:2])) +py_version = '.'.join(map(str, sys.version_info[:2])) site_pkgs_path = 'lib/python' + py_version + '/site-packages/rmf_demos_panel' setup( @@ -11,17 +12,16 @@ version='2.1.2', packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), ( - site_pkgs_path + '/templates', - glob(package_name + '/templates/*') + 'share/ament_index/resource_index/packages', + ['resource/' + package_name], ), + ('share/' + package_name, ['package.xml']), + (site_pkgs_path + '/templates', glob(package_name + '/templates/*')), ( site_pkgs_path + '/static/dist', - glob(package_name + '/static/dist/*.*') - ) + glob(package_name + '/static/dist/*.*'), + ), ], install_requires=['setuptools'], zip_safe=True, diff --git a/rmf_demos_tasks/rmf_demos_tasks/cancel_task.py b/rmf_demos_tasks/rmf_demos_tasks/cancel_task.py index 04f53e1d..009c5984 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/cancel_task.py +++ b/rmf_demos_tasks/rmf_demos_tasks/cancel_task.py @@ -14,32 +14,36 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import uuid import argparse import json +import sys +import uuid import rclpy from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.qos import qos_profile_system_default -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - from rmf_task_msgs.msg import ApiRequest - ############################################################################### -class TaskRequester(Node): +class TaskRequester(Node): def __init__(self, argv=sys.argv): super().__init__('task_requester') parser = argparse.ArgumentParser() - parser.add_argument('-id', '--task_id', required=True, default='', - type=str, help='Cancel Task ID') + parser.add_argument( + '-id', + '--task_id', + required=True, + default='', + type=str, + help='Cancel Task ID', + ) self.args = parser.parse_args(argv[1:]) @@ -47,22 +51,25 @@ def __init__(self, argv=sys.argv): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.pub = self.create_publisher( - ApiRequest, 'task_api_requests', transient_qos) + ApiRequest, 'task_api_requests', transient_qos + ) # Construct task msg = ApiRequest() - msg.request_id = "cancel_task_" + str(uuid.uuid4()) + msg.request_id = 'cancel_task_' + str(uuid.uuid4()) payload = {} - payload["type"] = "cancel_task_request" - payload["task_id"] = self.args.task_id + payload['type'] = 'cancel_task_request' + payload['task_id'] = self.args.task_id msg.json_msg = json.dumps(payload) - print(f"msg: \n{json.dumps(payload, indent=2)}") + print(f'msg: \n{json.dumps(payload, indent=2)}') self.pub.publish(msg) + ############################################################################### diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py index 93368658..e8cc4f9d 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py @@ -14,53 +14,86 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import uuid import argparse -import json import asyncio +import json +import sys +import uuid import rclpy from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.qos import qos_profile_system_default -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_task_msgs.msg import ApiRequest, ApiResponse - +from rmf_task_msgs.msg import ApiRequest +from rmf_task_msgs.msg import ApiResponse ############################################################################### -class TaskRequester(Node): +class TaskRequester(Node): def __init__(self, argv=sys.argv): super().__init__('task_requester') parser = argparse.ArgumentParser() - parser.add_argument('-s', '--starts', default=[], - type=str, nargs='+', help='Action start waypoints') - parser.add_argument('-st', '--start_time', - help='Start time from now in secs, default: 0', - type=int, default=0) - parser.add_argument('-pt', '--priority', - help='Priority value for this request', - type=int, default=0) - parser.add_argument('-a', '--action', required=True, - type=str, help='action name') - parser.add_argument('-ad', '--action_desc', required=False, - default='{}', - type=str, help='action description in json str') - parser.add_argument('-F', '--fleet', type=str, - help='Fleet name, should define tgt with robot') - parser.add_argument('-R', '--robot', type=str, - help='Robot name, should define tgt with fleet') - parser.add_argument("--use_sim_time", action="store_true", - help='Use sim time, default: false') - parser.add_argument("--use_tool_sink", action="store_true", - help='Use tool sink during perform action, \ - default: false') + parser.add_argument( + '-s', + '--starts', + default=[], + type=str, + nargs='+', + help='Action start waypoints', + ) + parser.add_argument( + '-st', + '--start_time', + help='Start time from now in secs, default: 0', + type=int, + default=0, + ) + parser.add_argument( + '-pt', + '--priority', + help='Priority value for this request', + type=int, + default=0, + ) + parser.add_argument( + '-a', '--action', required=True, type=str, help='action name' + ) + parser.add_argument( + '-ad', + '--action_desc', + required=False, + default='{}', + type=str, + help='action description in json str', + ) + parser.add_argument( + '-F', + '--fleet', + type=str, + help='Fleet name, should define tgt with robot', + ) + parser.add_argument( + '-R', + '--robot', + type=str, + help='Robot name, should define tgt with fleet', + ) + parser.add_argument( + '--use_sim_time', + action='store_true', + help='Use sim time, default: false', + ) + parser.add_argument( + '--use_tool_sink', + action='store_true', + help='Use tool sink during perform action, \ + default: false', + ) self.args = parser.parse_args(argv[1:]) self.response = asyncio.Future() @@ -69,80 +102,87 @@ def __init__(self, argv=sys.argv): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.pub = self.create_publisher( - ApiRequest, 'task_api_requests', transient_qos) + ApiRequest, 'task_api_requests', transient_qos + ) # enable ros sim time if self.args.use_sim_time: - self.get_logger().info("Using Sim Time") - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.get_logger().info('Using Sim Time') + param = Parameter('use_sim_time', Parameter.Type.BOOL, True) self.set_parameters([param]) # Construct task msg = ApiRequest() - msg.request_id = "custom_action_" + str(uuid.uuid4()) + msg.request_id = 'custom_action_' + str(uuid.uuid4()) payload = {} if self.args.fleet and self.args.robot: self.get_logger().info("Using 'robot_task_request'") - payload["type"] = "robot_task_request" - payload["robot"] = self.args.robot - payload["fleet"] = self.args.fleet + payload['type'] = 'robot_task_request' + payload['robot'] = self.args.robot + payload['fleet'] = self.args.fleet else: self.get_logger().info("Using 'dispatch_task_request'") - payload["type"] = "dispatch_task_request" + payload['type'] = 'dispatch_task_request' request = {} # Set task request start time now = self.get_clock().now().to_msg() now.sec = now.sec + self.args.start_time - start_time = now.sec * 1000 + round(now.nanosec/10**6) - request["unix_millis_earliest_start_time"] = start_time + start_time = now.sec * 1000 + round(now.nanosec / 10**6) + request['unix_millis_earliest_start_time'] = start_time # todo(YV): Fill priority after schema is added # Define task request category - request["category"] = "compose" + request['category'] = 'compose' + + if self.args.fleet: + request['fleet_name'] = self.args.fleet # Define task request description with phases description = {} # task_description_Compose.json - description["category"] = self.args.action - description["phases"] = [] + description['category'] = self.args.action + description['phases'] = [] activities = [] def _add_action(): activities.append( { - "category": "perform_action", - "description": - { - "unix_millis_action_duration_estimate": 60000, - "category": self.args.action, - "description": json.loads(self.args.action_desc), - "use_tool_sink": self.args.use_tool_sink - } - }) + 'category': 'perform_action', + 'description': { + 'unix_millis_action_duration_estimate': 60000, + 'category': self.args.action, + 'description': json.loads(self.args.action_desc), + 'use_tool_sink': self.args.use_tool_sink, + }, + } + ) if not self.args.starts: _add_action() else: # Add action activities for start in self.args.starts: - activities.append({ - "category": "go_to_place", - "description": start - }) + activities.append( + {'category': 'go_to_place', 'description': start} + ) _add_action() # Add activities to phases - description["phases"].append({ - "activity": { - "category": "sequence", - "description": {"activities": activities}} - }) - - request["description"] = description - payload["request"] = request + description['phases'].append( + { + 'activity': { + 'category': 'sequence', + 'description': {'activities': activities}, + } + } + ) + + request['description'] = description + payload['request'] = request msg.json_msg = json.dumps(payload) def receive_response(response_msg: ApiResponse): @@ -153,7 +193,7 @@ def receive_response(response_msg: ApiResponse): ApiResponse, 'task_api_responses', receive_response, 10 ) - print(f"msg: \n{json.dumps(payload, indent=2)}") + print(f'msg: \n{json.dumps(payload, indent=2)}') self.pub.publish(msg) @@ -166,7 +206,8 @@ def main(argv=sys.argv): task_requester = TaskRequester(args_without_ros) rclpy.spin_until_future_complete( - task_requester, task_requester.response, timeout_sec=5.0) + task_requester, task_requester.response, timeout_sec=5.0 + ) if task_requester.response.done(): print(f'Got response:\n{task_requester.response.result()}') else: diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py index a7549405..5cc91bc3 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py @@ -14,45 +14,68 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import uuid import argparse -import json import asyncio +import json +import sys +import uuid import rclpy from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.qos import qos_profile_system_default -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_task_msgs.msg import ApiRequest, ApiResponse - +from rmf_task_msgs.msg import ApiRequest +from rmf_task_msgs.msg import ApiResponse ############################################################################### -class TaskRequester(Node): +class TaskRequester(Node): def __init__(self, argv=sys.argv): super().__init__('task_requester') parser = argparse.ArgumentParser() - parser.add_argument('-cs', '--clean_start', required=True, - type=str, help='Cleaning zone') - parser.add_argument('-F', '--fleet', type=str, - help='Fleet name, should define tgt with robot') - parser.add_argument('-R', '--robot', type=str, - help='Robot name, should define tgt with fleet') - parser.add_argument('-st', '--start_time', - help='Start time from now in secs, default: 0', - type=int, default=0) - parser.add_argument('-pt', '--priority', - help='Priority value for this request', - type=int, default=0) - parser.add_argument("--use_sim_time", action="store_true", - help='Use sim time, default: false') + parser.add_argument( + '-cs', + '--clean_start', + required=True, + type=str, + help='Cleaning zone', + ) + parser.add_argument( + '-F', + '--fleet', + type=str, + help='Fleet name, should define tgt with robot', + ) + parser.add_argument( + '-R', + '--robot', + type=str, + help='Robot name, should define tgt with fleet', + ) + parser.add_argument( + '-st', + '--start_time', + help='Start time from now in secs, default: 0', + type=int, + default=0, + ) + parser.add_argument( + '-pt', + '--priority', + help='Priority value for this request', + type=int, + default=0, + ) + parser.add_argument( + '--use_sim_time', + action='store_true', + help='Use sim time, default: false', + ) self.args = parser.parse_args(argv[1:]) self.response = asyncio.Future() @@ -61,75 +84,80 @@ def __init__(self, argv=sys.argv): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.pub = self.create_publisher( - ApiRequest, 'task_api_requests', transient_qos) + ApiRequest, 'task_api_requests', transient_qos + ) # enable ros sim time if self.args.use_sim_time: - self.get_logger().info("Using Sim Time") - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.get_logger().info('Using Sim Time') + param = Parameter('use_sim_time', Parameter.Type.BOOL, True) self.set_parameters([param]) # Construct task msg = ApiRequest() - msg.request_id = "clean_" + str(uuid.uuid4()) + msg.request_id = 'clean_' + str(uuid.uuid4()) payload = {} if self.args.fleet and self.args.robot: self.get_logger().info("Using 'robot_task_request'") - payload["type"] = "robot_task_request" - payload["robot"] = self.args.robot - payload["fleet"] = self.args.fleet + payload['type'] = 'robot_task_request' + payload['robot'] = self.args.robot + payload['fleet'] = self.args.fleet else: self.get_logger().info("Using 'dispatch_task_request'") - payload["type"] = "dispatch_task_request" + payload['type'] = 'dispatch_task_request' request = {} # Set task request start time now = self.get_clock().now().to_msg() now.sec = now.sec + self.args.start_time - start_time = now.sec * 1000 + round(now.nanosec/10**6) - request["unix_millis_earliest_start_time"] = start_time + start_time = now.sec * 1000 + round(now.nanosec / 10**6) + request['unix_millis_earliest_start_time'] = start_time # Define task request category - request["category"] = "compose" + request['category'] = 'compose' + + if self.args.fleet: + request['fleet_name'] = self.args.fleet # Define task request description with cleaning zone description = {} # task_description_Compose.json - description["category"] = "clean" - description["phases"] = [] + description['category'] = 'clean' + description['phases'] = [] activities = [] # Send robot to start waypoint first - activities.append({ - "category": "go_to_place", - "description": self.args.clean_start - }) + activities.append( + {'category': 'go_to_place', 'description': self.args.clean_start} + ) # Send a perform action request for robot to clean area - activities.append({ - "category": "perform_action", - "description": + activities.append( { - "unix_millis_action_duration_estimate": 60000, - "category": "clean", - "expected_finish_location": self.args.clean_start, - "description": - { - "zone": self.args.clean_start + 'category': 'perform_action', + 'description': { + 'unix_millis_action_duration_estimate': 60000, + 'category': 'clean', + 'expected_finish_location': self.args.clean_start, + 'description': {'zone': self.args.clean_start}, + 'use_tool_sink': True, }, - "use_tool_sink": True } - }) + ) - description["phases"].append({ - "activity": { - "category": "sequence", - "description": {"activities": activities}} - }) + description['phases'].append( + { + 'activity': { + 'category': 'sequence', + 'description': {'activities': activities}, + } + } + ) - request["description"] = description - payload["request"] = request + request['description'] = description + payload['request'] = request msg.json_msg = json.dumps(payload) def receive_response(response_msg: ApiResponse): @@ -140,7 +168,7 @@ def receive_response(response_msg: ApiResponse): ApiResponse, 'task_api_responses', receive_response, 10 ) - print(f"Json msg payload: \n{json.dumps(payload, indent=2)}") + print(f'Json msg payload: \n{json.dumps(payload, indent=2)}') self.pub.publish(msg) @@ -153,7 +181,8 @@ def main(argv=sys.argv): task_requester = TaskRequester(args_without_ros) rclpy.spin_until_future_complete( - task_requester, task_requester.response, timeout_sec=5.0) + task_requester, task_requester.response, timeout_sec=5.0 + ) if task_requester.response.done(): print(f'Got response:\n{task_requester.response.result()}') else: diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py index 9035d683..7ec00227 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py @@ -14,74 +14,124 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import uuid import argparse -import json import asyncio +import json +import sys +import uuid import rclpy from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.qos import qos_profile_system_default -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_task_msgs.msg import ApiRequest, ApiResponse - +from rmf_task_msgs.msg import ApiRequest +from rmf_task_msgs.msg import ApiResponse ############################################################################### -class TaskRequester(Node): +class TaskRequester(Node): def __init__(self, argv=sys.argv): super().__init__('task_requester') parser = argparse.ArgumentParser() - parser.add_argument('-p', '--pickups', required=True, - type=str, nargs='+', - help="Pickup names") - parser.add_argument('-d', '--dropoffs', required=True, - type=str, nargs='+', - help="Dropoff names") - parser.add_argument('-ph', '--pickup_handlers', required=True, - type=str, nargs='+', - help="Pickup handler names") - parser.add_argument('-dh', '--dropoff_handlers', required=True, - type=str, nargs='+', - help="Dropoffs handler names") - parser.add_argument('-pp', '--pickup_payloads', - type=str, nargs='+', default=[], - help="Pickup payload [sku,quantity sku2,qty...]") - parser.add_argument('-dp', '--dropoff_payloads', - type=str, nargs='+', default=[], - help="Dropoff payload [sku,quantity sku2,qty...]") - parser.add_argument('-F', '--fleet', type=str, - help='Fleet name, should define tgt with robot') - parser.add_argument('-R', '--robot', type=str, - help='Robot name, should define tgt with fleet') - parser.add_argument('-st', '--start_time', - help='Start time from now in secs, default: 0', - type=int, default=0) - parser.add_argument('-pt', '--priority', - help='Priority value for this request', - type=int, default=0) - parser.add_argument("--use_sim_time", action="store_true", - help='Use sim time, default: false') + parser.add_argument( + '-p', + '--pickups', + required=True, + type=str, + nargs='+', + help='Pickup names', + ) + parser.add_argument( + '-d', + '--dropoffs', + required=True, + type=str, + nargs='+', + help='Dropoff names', + ) + parser.add_argument( + '-ph', + '--pickup_handlers', + required=True, + type=str, + nargs='+', + help='Pickup handler names', + ) + parser.add_argument( + '-dh', + '--dropoff_handlers', + required=True, + type=str, + nargs='+', + help='Dropoffs handler names', + ) + parser.add_argument( + '-pp', + '--pickup_payloads', + type=str, + nargs='+', + default=[], + help='Pickup payload [sku,quantity sku2,qty...]', + ) + parser.add_argument( + '-dp', + '--dropoff_payloads', + type=str, + nargs='+', + default=[], + help='Dropoff payload [sku,quantity sku2,qty...]', + ) + parser.add_argument( + '-F', + '--fleet', + type=str, + help='Fleet name, should define tgt with robot', + ) + parser.add_argument( + '-R', + '--robot', + type=str, + help='Robot name, should define tgt with fleet', + ) + parser.add_argument( + '-st', + '--start_time', + help='Start time from now in secs, default: 0', + type=int, + default=0, + ) + parser.add_argument( + '-pt', + '--priority', + help='Priority value for this request', + type=int, + default=0, + ) + parser.add_argument( + '--use_sim_time', + action='store_true', + help='Use sim time, default: false', + ) self.args = parser.parse_args(argv[1:]) self.response = asyncio.Future() # check user delivery arg inputs - if (len(self.args.pickups) != len(self.args.pickup_handlers)): + if len(self.args.pickups) != len(self.args.pickup_handlers): self.get_logger().error( - "Invalid pickups, [-p] should have the same length as [-ph]") + 'Invalid pickups, [-p] should have the same length as [-ph]' + ) parser.print_help() sys.exit(1) - if (len(self.args.dropoffs) != len(self.args.dropoff_handlers)): + if len(self.args.dropoffs) != len(self.args.dropoff_handlers): self.get_logger().error( - "Invalid dropoffs, [-d] should have the same length as [-dh]") + 'Invalid dropoffs, [-d] should have the same length as [-dh]' + ) parser.print_help() sys.exit(1) @@ -89,103 +139,118 @@ def __init__(self, argv=sys.argv): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.pub = self.create_publisher( - ApiRequest, 'task_api_requests', transient_qos) + ApiRequest, 'task_api_requests', transient_qos + ) # enable ros sim time if self.args.use_sim_time: - self.get_logger().info("Using Sim Time") - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.get_logger().info('Using Sim Time') + param = Parameter('use_sim_time', Parameter.Type.BOOL, True) self.set_parameters([param]) # Construct task msg = ApiRequest() - msg.request_id = "delivery_" + str(uuid.uuid4()) + msg.request_id = 'delivery_' + str(uuid.uuid4()) payload = {} if self.args.fleet and self.args.robot: self.get_logger().info("Using 'robot_task_request'") - payload["type"] = "robot_task_request" - payload["robot"] = self.args.robot - payload["fleet"] = self.args.fleet + payload['type'] = 'robot_task_request' + payload['robot'] = self.args.robot + payload['fleet'] = self.args.fleet else: self.get_logger().info("Using 'dispatch_task_request'") - payload["type"] = "dispatch_task_request" + payload['type'] = 'dispatch_task_request' request = {} # Set task request start time now = self.get_clock().now().to_msg() now.sec = now.sec + self.args.start_time - start_time = now.sec * 1000 + round(now.nanosec/10**6) - request["unix_millis_earliest_start_time"] = start_time + start_time = now.sec * 1000 + round(now.nanosec / 10**6) + request['unix_millis_earliest_start_time'] = start_time + + if self.args.fleet: + request['fleet_name'] = self.args.fleet def __create_pickup_desc(index): if index < len(self.args.pickup_payloads): sku_qty = self.args.pickup_payloads[index].split(',') - assert len(sku_qty) == 2, \ - "please specify sku and qty for pickup payload" - payload = [{"sku": sku_qty[0], - "quantity": int(sku_qty[1])}] + assert ( + len(sku_qty) == 2 + ), 'please specify sku and qty for pickup payload' + payload = [{'sku': sku_qty[0], 'quantity': int(sku_qty[1])}] else: payload = [] return { - "place": self.args.pickups[index], - "handler": self.args.pickup_handlers[index], - "payload": payload - } + 'place': self.args.pickups[index], + 'handler': self.args.pickup_handlers[index], + 'payload': payload, + } def __create_dropoff_desc(index): if index < len(self.args.dropoff_payloads): sku_qty = self.args.dropoff_payloads[index].split(',') - assert len(sku_qty) == 2, \ - "please specify sku and qty for dropoff payload" - payload = [{"sku": sku_qty[0], - "quantity": int(sku_qty[1])}] + assert ( + len(sku_qty) == 2 + ), 'please specify sku and qty for dropoff payload' + payload = [{'sku': sku_qty[0], 'quantity': int(sku_qty[1])}] else: payload = [] return { - "place": self.args.dropoffs[index], - "handler": self.args.dropoff_handlers[index], - "payload": payload - } + 'place': self.args.dropoffs[index], + 'handler': self.args.dropoff_handlers[index], + 'payload': payload, + } # Use standard delivery task type if len(self.args.pickups) == 1 and len(self.args.dropoffs) == 1: - request["category"] = "delivery" + request['category'] = 'delivery' description = { - "pickup": __create_pickup_desc(0), - "dropoff": __create_dropoff_desc(0) - } + 'pickup': __create_pickup_desc(0), + 'dropoff': __create_dropoff_desc(0), + } else: # Define multi_delivery with request category compose - request["category"] = "compose" + request['category'] = 'compose' # Define task request description with phases description = {} # task_description_Compose.json - description["category"] = "multi_delivery" - description["phases"] = [] + description['category'] = 'multi_delivery' + description['phases'] = [] activities = [] # Add each pickup for i in range(0, len(self.args.pickups)): - activities.append({ - "category": "pickup", - "description": __create_pickup_desc(i)}) + activities.append( + { + 'category': 'pickup', + 'description': __create_pickup_desc(i), + } + ) # Add each dropoff for i in range(0, len(self.args.dropoffs)): - activities.append({ - "category": "dropoff", - "description": __create_dropoff_desc(i)}) + activities.append( + { + 'category': 'dropoff', + 'description': __create_dropoff_desc(i), + } + ) # Add activities to phases - description["phases"].append( - {"activity": { - "category": "sequence", - "description": {"activities": activities}}}) + description['phases'].append( + { + 'activity': { + 'category': 'sequence', + 'description': {'activities': activities}, + } + } + ) - request["description"] = description - payload["request"] = request + request['description'] = description + payload['request'] = request msg.json_msg = json.dumps(payload) def receive_response(response_msg: ApiResponse): @@ -196,7 +261,7 @@ def receive_response(response_msg: ApiResponse): ApiResponse, 'task_api_responses', receive_response, 10 ) - print(f"Json msg payload: \n{json.dumps(payload, indent=2)}") + print(f'Json msg payload: \n{json.dumps(payload, indent=2)}') self.pub.publish(msg) @@ -210,7 +275,8 @@ def main(argv=sys.argv): task_requester = TaskRequester(args_without_ros) rclpy.spin_until_future_complete( - task_requester, task_requester.response, timeout_sec=5.0) + task_requester, task_requester.response, timeout_sec=5.0 + ) if task_requester.response.done(): print(f'Got response:\n{task_requester.response.result()}') else: diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_go_to_place.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_go_to_place.py index 044d41f3..dd1d2b31 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_go_to_place.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_go_to_place.py @@ -14,29 +14,28 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import uuid import argparse -import json import asyncio +import json import math +import sys +import uuid import rclpy from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.qos import qos_profile_system_default -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_task_msgs.msg import ApiRequest, ApiResponse - +from rmf_task_msgs.msg import ApiRequest +from rmf_task_msgs.msg import ApiResponse ############################################################################### -class TaskRequester(Node): +class TaskRequester(Node): def __init__(self, argv=sys.argv): super().__init__('task_requester') parser = argparse.ArgumentParser() @@ -46,17 +45,31 @@ def __init__(self, argv=sys.argv): '-p', '--place', required=True, type=str, help='Place to go to' ) parser.add_argument( - '-o', '--orientation', required=False, type=float, - help='Orientation to face in degrees (optional)' + '-o', + '--orientation', + required=False, + type=float, + help='Orientation to face in degrees (optional)', + ) + parser.add_argument( + '-st', + '--start_time', + help='Start time from now in secs, default: 0', + type=int, + default=0, + ) + parser.add_argument( + '-pt', + '--priority', + help='Priority value for this request', + type=int, + default=0, + ) + parser.add_argument( + '--use_sim_time', + action='store_true', + help='Use sim time, default: false', ) - parser.add_argument('-st', '--start_time', - help='Start time from now in secs, default: 0', - type=int, default=0) - parser.add_argument('-pt', '--priority', - help='Priority value for this request', - type=int, default=0) - parser.add_argument("--use_sim_time", action="store_true", - help='Use sim time, default: false') self.args = parser.parse_args(argv[1:]) self.response = asyncio.Future() @@ -65,60 +78,64 @@ def __init__(self, argv=sys.argv): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.pub = self.create_publisher( - ApiRequest, 'task_api_requests', transient_qos + ApiRequest, 'task_api_requests', transient_qos ) # enable ros sim time if self.args.use_sim_time: - self.get_logger().info("Using Sim Time") - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.get_logger().info('Using Sim Time') + param = Parameter('use_sim_time', Parameter.Type.BOOL, True) self.set_parameters([param]) # Construct task msg = ApiRequest() - msg.request_id = "direct_" + str(uuid.uuid4()) + msg.request_id = 'direct_' + str(uuid.uuid4()) payload = {} if self.args.robot and self.args.fleet: self.get_logger().info("Using 'robot_task_request'") - payload["type"] = "robot_task_request" - payload["robot"] = self.args.robot - payload["fleet"] = self.args.fleet + payload['type'] = 'robot_task_request' + payload['robot'] = self.args.robot + payload['fleet'] = self.args.fleet else: self.get_logger().info("Using 'dispatch_task_request'") - payload["type"] = "dispatch_task_request" + payload['type'] = 'dispatch_task_request' # Set task request start time now = self.get_clock().now().to_msg() now.sec = now.sec + self.args.start_time - start_time = now.sec * 1000 + round(now.nanosec/10**6) + start_time = now.sec * 1000 + round(now.nanosec / 10**6) # todo(YV): Fill priority after schema is added # Define task request description go_to_description = {'waypoint': self.args.place} if self.args.orientation is not None: go_to_description['orientation'] = ( - self.args.orientation*math.pi/180.0 + self.args.orientation * math.pi / 180.0 ) go_to_activity = { 'category': 'go_to_place', - 'description': go_to_description + 'description': go_to_description, } rmf_task_request = { 'category': 'compose', 'description': { 'category': 'go_to_place', - 'phases': [{'activity': go_to_activity}] + 'phases': [{'activity': go_to_activity}], }, - 'unix_millis_earliest_start_time': start_time + 'unix_millis_earliest_start_time': start_time, } - payload["request"] = rmf_task_request + if self.args.fleet: + rmf_task_request['fleet_name'] = self.args.fleet + + payload['request'] = rmf_task_request msg.json_msg = json.dumps(payload) @@ -130,7 +147,7 @@ def receive_response(response_msg: ApiResponse): ApiResponse, 'task_api_responses', receive_response, 10 ) - print(f"Json msg payload: \n{json.dumps(payload, indent=2)}") + print(f'Json msg payload: \n{json.dumps(payload, indent=2)}') self.pub.publish(msg) @@ -144,7 +161,8 @@ def main(argv=sys.argv): task_requester = TaskRequester(args_without_ros) rclpy.spin_until_future_complete( - task_requester, task_requester.response, timeout_sec=5.0) + task_requester, task_requester.response, timeout_sec=5.0 + ) if task_requester.response.done(): print(f'Got response:\n{task_requester.response.result()}') else: diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_loop.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_loop.py index 2418bf45..4f677041 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_loop.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_loop.py @@ -14,52 +14,71 @@ # See the License for the specific language governing permissions and # limitations under the License. +import argparse import sys -import uuid import time -import argparse +import uuid import rclpy from rclpy.node import Node from rclpy.parameter import Parameter +from rmf_task_msgs.msg import Loop +from rmf_task_msgs.msg import TaskType from rmf_task_msgs.srv import SubmitTask -from rmf_task_msgs.msg import TaskType, Loop ############################################################################### class TaskRequester: - def __init__(self, argv=sys.argv): parser = argparse.ArgumentParser() - parser.add_argument('-s', '--start', required=True, - type=str, help='Start waypoint') - parser.add_argument('-f', '--finish', required=True, - type=str, help='Finish waypoint') - parser.add_argument('-n', '--loop_num', - help='Number of loops to perform', - type=int, default=1) - parser.add_argument('-st', '--start_time', - help='Start time from now in secs, default: 0', - type=int, default=0) - parser.add_argument('-pt', '--priority', - help='Priority value for this request', - type=int, default=0) - parser.add_argument("--use_sim_time", action="store_true", - help='Use sim time, default: false') + parser.add_argument( + '-s', '--start', required=True, type=str, help='Start waypoint' + ) + parser.add_argument( + '-f', '--finish', required=True, type=str, help='Finish waypoint' + ) + parser.add_argument( + '-n', + '--loop_num', + help='Number of loops to perform', + type=int, + default=1, + ) + parser.add_argument( + '-st', + '--start_time', + help='Start time from now in secs, default: 0', + type=int, + default=0, + ) + parser.add_argument( + '-pt', + '--priority', + help='Priority value for this request', + type=int, + default=0, + ) + parser.add_argument( + '--use_sim_time', + action='store_true', + help='Use sim time, default: false', + ) self.args = parser.parse_args(argv[1:]) self.node = rclpy.create_node('task_requester') self.submit_task_srv = self.node.create_client( - SubmitTask, '/submit_task') + SubmitTask, '/submit_task' + ) self.node.get_logger().warn( - '[Deprecation warning] use `dispatch_patrol` instead') + '[Deprecation warning] use `dispatch_patrol` instead' + ) # enable ros sim time if self.args.use_sim_time: - self.node.get_logger().info("Using Sim Time") - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.node.get_logger().info('Using Sim Time') + param = Parameter('use_sim_time', Parameter.Type.BOOL, True) self.node.set_parameters([param]) def generate_task_req_msg(self): @@ -86,23 +105,26 @@ def main(self): rclpy.spin_once(self.node, timeout_sec=1.0) req_msg = self.generate_task_req_msg() - print(f"\nGenerated loop request: \n {req_msg}\n") - self.node.get_logger().info("Submitting Loop Request") + print(f'\nGenerated loop request: \n {req_msg}\n') + self.node.get_logger().info('Submitting Loop Request') try: future = self.submit_task_srv.call_async(req_msg) rclpy.spin_until_future_complete( - self.node, future, timeout_sec=1.0) + self.node, future, timeout_sec=1.0 + ) response = future.result() if response is None: self.node.get_logger().error('/submit_task srv call failed') elif not response.success: self.node.get_logger().error( - 'Dispatcher node failed to accept task') + 'Dispatcher node failed to accept task' + ) else: self.node.get_logger().info( 'Request was successfully submitted ' - f'and assigned task_id: [{response.task_id}]') + f'and assigned task_id: [{response.task_id}]' + ) except Exception as e: self.node.get_logger().error('Error! Submit Srv failed %r' % (e,)) diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py index d47ba9c4..b1273622 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py @@ -14,47 +14,75 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import uuid import argparse -import json import asyncio +import json +import sys +import uuid import rclpy from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_task_msgs.msg import ApiRequest, ApiResponse - +from rmf_task_msgs.msg import ApiRequest +from rmf_task_msgs.msg import ApiResponse ############################################################################### -class TaskRequester(Node): +class TaskRequester(Node): def __init__(self, argv=sys.argv): super().__init__('task_requester') parser = argparse.ArgumentParser() - parser.add_argument('-p', '--places', required=True, nargs='+', - type=str, help='Places to patrol through') - parser.add_argument('-n', '--rounds', - help='Number of loops to perform', - type=int, default=1) - parser.add_argument('-F', '--fleet', type=str, - help='Fleet name, should define tgt with robot') - parser.add_argument('-R', '--robot', type=str, - help='Robot name, should define tgt with fleet') - parser.add_argument('-st', '--start_time', - help='Start time from now in secs, default: 0', - type=int, default=0) - parser.add_argument('-pt', '--priority', - help='Priority value for this request', - type=int, default=0) - parser.add_argument("--use_sim_time", action="store_true", - help='Use sim time, default: false') + parser.add_argument( + '-p', + '--places', + required=True, + nargs='+', + type=str, + help='Places to patrol through', + ) + parser.add_argument( + '-n', + '--rounds', + help='Number of loops to perform', + type=int, + default=1, + ) + parser.add_argument( + '-F', + '--fleet', + type=str, + help='Fleet name, should define tgt with robot', + ) + parser.add_argument( + '-R', + '--robot', + type=str, + help='Robot name, should define tgt with fleet', + ) + parser.add_argument( + '-st', + '--start_time', + help='Start time from now in secs, default: 0', + type=int, + default=0, + ) + parser.add_argument( + '-pt', + '--priority', + help='Priority value for this request', + type=int, + default=0, + ) + parser.add_argument( + '--use_sim_time', + action='store_true', + help='Use sim time, default: false', + ) self.args = parser.parse_args(argv[1:]) self.response = asyncio.Future() @@ -63,50 +91,51 @@ def __init__(self, argv=sys.argv): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.pub = self.create_publisher( - ApiRequest, 'task_api_requests', transient_qos + ApiRequest, 'task_api_requests', transient_qos ) # enable ros sim time if self.args.use_sim_time: - self.get_logger().info("Using Sim Time") - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.get_logger().info('Using Sim Time') + param = Parameter('use_sim_time', Parameter.Type.BOOL, True) self.set_parameters([param]) # Construct task msg = ApiRequest() - msg.request_id = "patrol_" + str(uuid.uuid4()) + msg.request_id = 'patrol_' + str(uuid.uuid4()) payload = {} if self.args.robot and self.args.fleet: self.get_logger().info("Using 'robot_task_request'") - payload["type"] = "robot_task_request" - payload["robot"] = self.args.robot - payload["fleet"] = self.args.fleet + payload['type'] = 'robot_task_request' + payload['robot'] = self.args.robot + payload['fleet'] = self.args.fleet else: self.get_logger().info("Using 'dispatch_task_request'") - payload["type"] = "dispatch_task_request" + payload['type'] = 'dispatch_task_request' request = {} # Set task request start time now = self.get_clock().now().to_msg() now.sec = now.sec + self.args.start_time - start_time = now.sec * 1000 + round(now.nanosec/10**6) - request["unix_millis_earliest_start_time"] = start_time + start_time = now.sec * 1000 + round(now.nanosec / 10**6) + request['unix_millis_earliest_start_time'] = start_time # todo(YV): Fill priority after schema is added # Define task request category - request["category"] = "patrol" + request['category'] = 'patrol' + + if self.args.fleet: + request['fleet_name'] = self.args.fleet # Define task request description - description = { - 'places': self.args.places, - 'rounds': self.args.rounds - } - request["description"] = description - payload["request"] = request + description = {'places': self.args.places, 'rounds': self.args.rounds} + request['description'] = description + payload['request'] = request msg.json_msg = json.dumps(payload) def receive_response(response_msg: ApiResponse): @@ -118,7 +147,7 @@ def receive_response(response_msg: ApiResponse): ApiResponse, 'task_api_responses', receive_response, transient_qos ) - print(f"Json msg payload: \n{json.dumps(payload, indent=2)}") + print(f'Json msg payload: \n{json.dumps(payload, indent=2)}') self.pub.publish(msg) @@ -131,7 +160,8 @@ def main(argv=sys.argv): task_requester = TaskRequester(args_without_ros) rclpy.spin_until_future_complete( - task_requester, task_requester.response, timeout_sec=5.0) + task_requester, task_requester.response, timeout_sec=5.0 + ) if task_requester.response.done(): print(f'Got response:\n{task_requester.response.result()}') else: diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_teleop.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_teleop.py index 8cb7196a..d5ac41f8 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_teleop.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_teleop.py @@ -14,45 +14,58 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import uuid import argparse -import json import asyncio +import json +import sys +import uuid import rclpy from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.qos import qos_profile_system_default -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_task_msgs.msg import ApiRequest, ApiResponse - +from rmf_task_msgs.msg import ApiRequest +from rmf_task_msgs.msg import ApiResponse ############################################################################### -class TaskRequester(Node): +class TaskRequester(Node): def __init__(self, argv=sys.argv): super().__init__('task_requester') parser = argparse.ArgumentParser() - parser.add_argument('-F', '--fleet', required=False, default='', - type=str, help='Fleet name') - parser.add_argument('-R', '--robot', required=False, default='', - type=str, help='Robot name') - parser.add_argument('-s', '--start', required=True, - type=str, help='Start waypoint') - parser.add_argument('-st', '--start_time', - help='Start time from now in secs, default: 0', - type=int, default=0) - parser.add_argument('-pt', '--priority', - help='Priority value for this request', - type=int, default=0) - parser.add_argument("--use_sim_time", action="store_true", - help='Use sim time, default: false') + parser.add_argument( + '-F', '--fleet', required=False, type=str, help='Fleet name' + ) + parser.add_argument( + '-R', '--robot', required=False, type=str, help='Robot name' + ) + parser.add_argument( + '-s', '--start', required=True, type=str, help='Start waypoint' + ) + parser.add_argument( + '-st', + '--start_time', + help='Start time from now in secs, default: 0', + type=int, + default=0, + ) + parser.add_argument( + '-pt', + '--priority', + help='Priority value for this request', + type=int, + default=0, + ) + parser.add_argument( + '--use_sim_time', + action='store_true', + help='Use sim time, default: false', + ) self.args = parser.parse_args(argv[1:]) self.response = asyncio.Future() @@ -61,57 +74,74 @@ def __init__(self, argv=sys.argv): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.pub = self.create_publisher( - ApiRequest, 'task_api_requests', transient_qos) + ApiRequest, 'task_api_requests', transient_qos + ) # enable ros sim time if self.args.use_sim_time: - self.get_logger().info("Using Sim Time") - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.get_logger().info('Using Sim Time') + param = Parameter('use_sim_time', Parameter.Type.BOOL, True) self.set_parameters([param]) # Construct task msg = ApiRequest() - msg.request_id = "teleop_" + str(uuid.uuid4()) + msg.request_id = 'teleop_' + str(uuid.uuid4()) payload = {} if self.args.fleet and self.args.robot: - payload["type"] = "robot_task_request" - payload["robot"] = self.args.robot - payload["fleet"] = self.args.fleet + payload['type'] = 'robot_task_request' + payload['robot'] = self.args.robot + payload['fleet'] = self.args.fleet else: - payload["type"] = "dispatch_task_request" + payload['type'] = 'dispatch_task_request' request = {} # Set task request start time now = self.get_clock().now().to_msg() now.sec = now.sec + self.args.start_time - start_time = now.sec * 1000 + round(now.nanosec/10**6) - request["unix_millis_earliest_start_time"] = start_time + start_time = now.sec * 1000 + round(now.nanosec / 10**6) + request['unix_millis_earliest_start_time'] = start_time # todo(YV): Fill priority after schema is added # Define task request category - request["category"] = "compose" + request['category'] = 'compose' + + if self.args.fleet: + request['fleet_name'] = self.args.fleet # Define task request description with phases description = {} # task_description_Compose.json - description["category"] = "teleop" - description["phases"] = [] + description['category'] = 'teleop' + description['phases'] = [] activities = [] # Add activities - activities.append({"category": "go_to_place", - "description": self.args.start}) - activities.append({"category": "perform_action", - "description": { - "unix_millis_action_duration_estimate": 60000, - "category": "teleop", "description": {}}}) + activities.append( + {'category': 'go_to_place', 'description': self.args.start} + ) + activities.append( + { + 'category': 'perform_action', + 'description': { + 'unix_millis_action_duration_estimate': 60000, + 'category': 'teleop', + 'description': {}, + }, + } + ) # Add activities to phases - description["phases"].append( - {"activity": {"category": "sequence", - "description": {"activities": activities}}}) - request["description"] = description - payload["request"] = request + description['phases'].append( + { + 'activity': { + 'category': 'sequence', + 'description': {'activities': activities}, + } + } + ) + request['description'] = description + payload['request'] = request msg.json_msg = json.dumps(payload) def receive_response(response_msg: ApiResponse): @@ -123,7 +153,7 @@ def receive_response(response_msg: ApiResponse): ApiResponse, 'task_api_responses', receive_response, transient_qos ) - print(f"Json msg payload: \n{json.dumps(payload, indent=2)}") + print(f'Json msg payload: \n{json.dumps(payload, indent=2)}') self.pub.publish(msg) @@ -136,7 +166,8 @@ def main(argv=sys.argv): task_requester = TaskRequester(args_without_ros) rclpy.spin_until_future_complete( - task_requester, task_requester.response, timeout_sec=5.0) + task_requester, task_requester.response, timeout_sec=5.0 + ) if task_requester.response.done(): print(f'Got response:\n{task_requester.response.result()}') else: diff --git a/rmf_demos_tasks/rmf_demos_tasks/mock_docker.py b/rmf_demos_tasks/rmf_demos_tasks/mock_docker.py index 4f24bc34..17cfed54 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/mock_docker.py +++ b/rmf_demos_tasks/rmf_demos_tasks/mock_docker.py @@ -1,4 +1,3 @@ - # Copyright 2021 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,22 +12,27 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import rclpy -import math import argparse -import yaml +import math +import sys import time -from rclpy.node import Node -from rclpy.time import Time -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History +import rclpy +from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability - -from rmf_fleet_msgs.msg import ModeRequest, PathRequest, Location, \ - RobotState, RobotMode, DockSummary, Dock, DockParameter +from rclpy.time import Time +from rmf_fleet_msgs.msg import Dock +from rmf_fleet_msgs.msg import DockParameter +from rmf_fleet_msgs.msg import DockSummary +from rmf_fleet_msgs.msg import Location +from rmf_fleet_msgs.msg import ModeRequest +from rmf_fleet_msgs.msg import PathRequest +from rmf_fleet_msgs.msg import RobotMode +from rmf_fleet_msgs.msg import RobotState +import yaml def make_location(p, level_name): @@ -71,24 +75,30 @@ def __init__(self, config_yaml): self.get_logger().info(f'Greetings, I am mock docker') self.config_yaml = config_yaml self.path_request_publisher = self.create_publisher( - PathRequest, 'robot_path_requests', 1) + PathRequest, 'robot_path_requests', 1 + ) self.mode_request_publisher = self.create_publisher( - ModeRequest, 'robot_mode_requests', 1) + ModeRequest, 'robot_mode_requests', 1 + ) self.mode_request_subscription = self.create_subscription( - ModeRequest, 'robot_mode_requests', self.mode_request_cb, 10) + ModeRequest, 'robot_mode_requests', self.mode_request_cb, 10 + ) self.robot_state_subscription = self.create_subscription( - RobotState, 'robot_state', self.robot_state_cb, 10) + RobotState, 'robot_state', self.robot_state_cb, 10 + ) transient_qos = QoSProfile( history=History.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=Reliability.RMW_QOS_POLICY_RELIABILITY_RELIABLE, - durability=Durability.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) + durability=Durability.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + ) self.dock_summary_publisher = self.create_publisher( - DockSummary, 'dock_summary', qos_profile=transient_qos) + DockSummary, 'dock_summary', qos_profile=transient_qos + ) # This is a dict of robots which are in docking mode self.watching = {} @@ -103,14 +113,15 @@ def __init__(self, config_yaml): for dock_name, dock_waypoints in docking_info.items(): param = DockParameter() param.start = dock_name - finish_waypoint = dock_waypoints.get("finish_waypoint") + finish_waypoint = dock_waypoints.get('finish_waypoint') if finish_waypoint is None: # for backwards compatibility finish_waypoint = dock_name param.finish = finish_waypoint - for point in dock_waypoints["path"]: + for point in dock_waypoints['path']: location = make_location( - point, dock_waypoints["level_name"]) + point, dock_waypoints['level_name'] + ) param.path.append(location) dock.params.append(param) dock_sub_map[dock_name] = param.path @@ -125,28 +136,33 @@ def mode_request_cb(self, msg: ModeRequest): if not msg.parameters: self.get_logger().warn( - f'Missing docking name for docking request!') + f'Missing docking name for docking request!' + ) return if msg.parameters[0].name != 'docking': self.get_logger().warn( - f'Unexpected docking parameter [{msg.parameters[0]}]') + f'Unexpected docking parameter [{msg.parameters[0]}]' + ) return fleet_name = self.dock_map.get(msg.fleet_name) if fleet_name is None: self.get_logger().warn( - 'Unknown fleet name requested [{msg.fleet_name}].') + 'Unknown fleet name requested [{msg.fleet_name}].' + ) return dock = fleet_name.get(msg.parameters[0].value) if not dock: self.get_logger().warn( - f'Unknown dock name requested [{msg.parameters[0].value}]') + f'Unknown dock name requested [{msg.parameters[0].value}]' + ) return self.get_logger().info( - f'Received Docking Mode Request from [{msg.robot_name}]') + f'Received Docking Mode Request from [{msg.robot_name}]' + ) path_request = PathRequest() path_request.fleet_name = msg.fleet_name path_request.robot_name = msg.robot_name @@ -180,22 +196,24 @@ def robot_state_cb(self, msg: RobotState): if msg.mode.mode != RobotMode.MODE_DOCKING: self.watching.pop(robot_name) self.get_logger().info( - f'{robot_name} done with docking at {finish_location}') + f'{robot_name} done with docking at {finish_location}' + ) def main(argv=sys.argv): rclpy.init(args=argv) args_without_ros = rclpy.utilities.remove_ros_args(argv) parser = argparse.ArgumentParser( - prog="mock_docker", - description="Configure and start mock_docker node") - parser.add_argument("-c", "--config", type=str, required=True, - help="Path to config file") + prog='mock_docker', description='Configure and start mock_docker node' + ) + parser.add_argument( + '-c', '--config', type=str, required=True, help='Path to config file' + ) args = parser.parse_args(args_without_ros[1:]) config = args.config - with open(config, "r") as f: + with open(config, 'r') as f: config_yaml = yaml.safe_load(f) mock_docker = MockDocker(config_yaml) diff --git a/rmf_demos_tasks/rmf_demos_tasks/request_lift.py b/rmf_demos_tasks/rmf_demos_tasks/request_lift.py index b56dea36..55b26cb9 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/request_lift.py +++ b/rmf_demos_tasks/rmf_demos_tasks/request_lift.py @@ -4,17 +4,20 @@ import uuid import rclpy - from rmf_lift_msgs.msg import LiftRequest def print_instructions(): - print("Invalid number of arguments, please pass in lift_name, desired ", - "level and door state after the script in that order, only supports", - "'open' or 'closed'.") - print('For example:\n', - ' request_lift Lift1 L1 open\n', - ' request_lift Lift2 L3 closed') + print( + 'Invalid number of arguments, please pass in lift_name, desired ', + 'level and door state after the script in that order, only supports', + "'open' or 'closed'.", + ) + print( + 'For example:\n', + ' request_lift Lift1 L1 open\n', + ' request_lift Lift2 L3 closed', + ) def main(argv=sys.argv): @@ -47,9 +50,11 @@ def main(argv=sys.argv): rclpy.shutdown() - print(f'Sent 5 messages at 2 Hz requesting lift: {argv[1]}, ', - f'to floor: {argv[2]}, ', - f'with door: {argv[3]}.') + print( + f'Sent 5 messages at 2 Hz requesting lift: {argv[1]}, ', + f'to floor: {argv[2]}, ', + f'with door: {argv[3]}.', + ) if __name__ == '__main__': diff --git a/rmf_demos_tasks/rmf_demos_tasks/request_loop.py b/rmf_demos_tasks/rmf_demos_tasks/request_loop.py index 0528a7ff..093404de 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/request_loop.py +++ b/rmf_demos_tasks/rmf_demos_tasks/request_loop.py @@ -14,32 +14,39 @@ # See the License for the specific language governing permissions and # limitations under the License. +import argparse import sys -import uuid import time -import argparse +import uuid import rclpy from rclpy.node import Node -from rmf_task_msgs.msg import Loop from rmf_fleet_msgs.msg import FleetState +from rmf_task_msgs.msg import Loop class LoopRequester: - def __init__(self, argv=sys.argv): parser = argparse.ArgumentParser() parser.add_argument('-s', '--start', help='Start waypoint') parser.add_argument('-f', '--finish', help='Finish waypoint') - parser.add_argument('-n', '--num', help='Number of loops to perform', - type=int, default=1) - parser.add_argument('-i', '--task-id', help='Task ID', default='', - type=str) + parser.add_argument( + '-n', + '--num', + help='Number of loops to perform', + type=int, + default=1, + ) + parser.add_argument( + '-i', '--task-id', help='Task ID', default='', type=str + ) parser.add_argument('-r', '--robot-type', help='Fleet name') parser.add_argument( '--delay', help='Number of secs to wait before sending out the request', - default=0, type=int) + default=0, + type=int, + ) args = parser.parse_args(argv[1:]) self.start_wp = args.start @@ -58,8 +65,9 @@ def main(self): request.start_name = self.start_wp request.finish_name = self.finish_wp request.num_loops = self.num_loops - request.task_id = self.task_id if self.task_id \ - else 'loop#' + str(uuid.uuid1()) + request.task_id = ( + self.task_id if self.task_id else 'loop#' + str(uuid.uuid1()) + ) time.sleep(self.num_sec_delay) self.publisher.publish(request) @@ -67,8 +75,11 @@ def main(self): rclpy.shutdown() self.node.get_logger().info( - 'Loop request between {} and {}, submitted to {} robot fleet' - .format(self.start_wp, self.finish_wp, self.robot_type)) + 'Loop request between {} and {}, submitted to {} robot ' + 'fleet'.format( + self.start_wp, self.finish_wp, self.robot_type + ) + ) def main(argv=sys.argv): diff --git a/rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py b/rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py index afa49d6d..6d193c7f 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py +++ b/rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py @@ -14,39 +14,46 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys -import uuid import argparse import asyncio +import sys +import uuid import rclpy from rclpy.node import Node -from rclpy.qos import QoSProfile -from rclpy.qos import QoSHistoryPolicy as History from rclpy.qos import QoSDurabilityPolicy as Durability +from rclpy.qos import QoSHistoryPolicy as History +from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy as Reliability from rclpy.timer import Timer - -from rmf_fleet_msgs.msg import PathRequest, Location - +from rmf_fleet_msgs.msg import Location +from rmf_fleet_msgs.msg import PathRequest ############################################################################### -class Requester(Node): +class Requester(Node): def __init__(self, argv=sys.argv): super().__init__('teleop_publisher') parser = argparse.ArgumentParser() - parser.add_argument('-F', '--fleet', required=True, - type=str, help='Fleet name') - parser.add_argument('-R', '--robot', required=True, - help='Robot name', type=str) - parser.add_argument('-p', '--points', required=True, nargs='+', - help='Coordinate points [x,y,yaw] for teleop', - type=str) - parser.add_argument('-m', '--map', required=True, - help='Map name', type=str) + parser.add_argument( + '-F', '--fleet', required=True, type=str, help='Fleet name' + ) + parser.add_argument( + '-R', '--robot', required=True, help='Robot name', type=str + ) + parser.add_argument( + '-p', + '--points', + required=True, + nargs='+', + help='Coordinate points [x,y,yaw] for teleop', + type=str, + ) + parser.add_argument( + '-m', '--map', required=True, help='Map name', type=str + ) self.args = parser.parse_args(argv[1:]) @@ -54,9 +61,11 @@ def __init__(self, argv=sys.argv): history=History.KEEP_LAST, depth=1, reliability=Reliability.RELIABLE, - durability=Durability.TRANSIENT_LOCAL) + durability=Durability.TRANSIENT_LOCAL, + ) self.pub = self.create_publisher( - PathRequest, 'robot_path_requests', transient_qos) + PathRequest, 'robot_path_requests', transient_qos + ) msg = PathRequest() msg.fleet_name = self.args.fleet @@ -64,7 +73,7 @@ def __init__(self, argv=sys.argv): msg.task_id = str(uuid.uuid1()) for p in self.args.points: pts = p.split(',') - assert len(pts) == 3, "each point should have 3 values {x,y,yaw}" + assert len(pts) == 3, 'each point should have 3 values {x,y,yaw}' loc = Location() loc.x = float(pts[0]) loc.y = float(pts[1]) @@ -73,6 +82,7 @@ def __init__(self, argv=sys.argv): msg.path.append(loc) self.pub.publish(msg) + ############################################################################### diff --git a/rmf_demos_tasks/setup.py b/rmf_demos_tasks/setup.py index 1d1a63a5..27d341e4 100644 --- a/rmf_demos_tasks/setup.py +++ b/rmf_demos_tasks/setup.py @@ -7,8 +7,10 @@ version='2.1.2', packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ( + 'share/ament_index/resource_index/packages', + ['resource/' + package_name], + ), ('share/' + package_name, ['package.xml']), ], install_requires=['setuptools'], @@ -22,18 +24,18 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'request_loop = rmf_demos_tasks.request_loop:main', - 'request_lift = rmf_demos_tasks.request_lift:main', - 'cancel_task = rmf_demos_tasks.cancel_task:main', - 'dispatch_loop = rmf_demos_tasks.dispatch_loop:main', - 'dispatch_action = rmf_demos_tasks.dispatch_action:main', - 'dispatch_patrol = rmf_demos_tasks.dispatch_patrol:main', - 'dispatch_delivery = rmf_demos_tasks.dispatch_delivery:main', - 'dispatch_clean = rmf_demos_tasks.dispatch_clean:main', - 'dispatch_go_to_place = rmf_demos_tasks.dispatch_go_to_place:main', - 'dispatch_teleop = rmf_demos_tasks.dispatch_teleop:main', - 'mock_docker = rmf_demos_tasks.mock_docker:main', - 'teleop_robot = rmf_demos_tasks.teleop_robot:main', + 'request_loop = rmf_demos_tasks.request_loop:main', + 'request_lift = rmf_demos_tasks.request_lift:main', + 'cancel_task = rmf_demos_tasks.cancel_task:main', + 'dispatch_loop = rmf_demos_tasks.dispatch_loop:main', + 'dispatch_action = rmf_demos_tasks.dispatch_action:main', + 'dispatch_patrol = rmf_demos_tasks.dispatch_patrol:main', + 'dispatch_delivery = rmf_demos_tasks.dispatch_delivery:main', + 'dispatch_clean = rmf_demos_tasks.dispatch_clean:main', + 'dispatch_go_to_place = rmf_demos_tasks.dispatch_go_to_place:main', + 'dispatch_teleop = rmf_demos_tasks.dispatch_teleop:main', + 'mock_docker = rmf_demos_tasks.mock_docker:main', + 'teleop_robot = rmf_demos_tasks.teleop_robot:main', ], }, )