From adfab0d9490bcccc80abdba5cc239c73e4e0cf78 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 27 Feb 2024 04:43:01 +0000 Subject: [PATCH 1/4] Add a script for dispatching tasks from a json description Signed-off-by: Michael X. Grey --- .../rmf_demos_tasks/dispatch_action.py | 4 +- .../rmf_demos_tasks/dispatch_clean.py | 4 +- .../rmf_demos_tasks/dispatch_delivery.py | 4 +- .../rmf_demos_tasks/dispatch_json.py | 165 ++++++++++++++++++ .../rmf_demos_tasks/dispatch_patrol.py | 4 +- 5 files changed, 173 insertions(+), 8 deletions(-) create mode 100644 rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py index 620e2629..5dce1ff6 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py @@ -75,13 +75,13 @@ def __init__(self, argv=sys.argv): '-F', '--fleet', type=str, - help='Fleet name, should define tgt with robot', + help='Fleet name, should define together with robot', ) parser.add_argument( '-R', '--robot', type=str, - help='Robot name, should define tgt with fleet', + help='Robot name, should define together with fleet', ) parser.add_argument( '--use_sim_time', diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py index 2172ecbd..4baac9fb 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py @@ -49,13 +49,13 @@ def __init__(self, argv=sys.argv): '-F', '--fleet', type=str, - help='Fleet name, should define tgt with robot', + help='Fleet name, should define together with robot', ) parser.add_argument( '-R', '--robot', type=str, - help='Robot name, should define tgt with fleet', + help='Robot name, should define together with fleet', ) parser.add_argument( '-st', diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py index a1555787..0f6ded28 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py @@ -90,13 +90,13 @@ def __init__(self, argv=sys.argv): '-F', '--fleet', type=str, - help='Fleet name, should define tgt with robot', + help='Fleet name, should define together with robot', ) parser.add_argument( '-R', '--robot', type=str, - help='Robot name, should define tgt with fleet', + help='Robot name, should define together with fleet', ) parser.add_argument( '-st', diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py new file mode 100644 index 00000000..4e4e5137 --- /dev/null +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import uuid +import argparse +import json +import asyncio + +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 QoSReliabilityPolicy as Reliability + +from rmf_task_msgs.msg import ApiRequest, ApiResponse + + +############################################################################### + +class TaskRequester(Node): + + def __init__(self, argv=sys.argv): + super().__init__('task_requester') + parser = argparse.ArgumentParser() + parser.add_argument( + '-c', '--category', + type=str, + default='compose', + help='Set the category of the task' + ) + parser.add_argument( + '-f', '--file', + required=True, + type=str, + help='File containing a task description formatted in json' + ) + parser.add_argument( + '-F', '--fleet', + type=str, + help='Fleet name, should define together with robot' + ) + parser.add_argument( + '-R', '--robot', + type=str, + help='Robot name, should define together with fleet' + ) + parser.add_argument( + '-st', '--start_time', + type=int, + default=0, + help='Start time from now in secs, default: 0' + ) + parser.add_argument( + '-pt', '--priority', + type=int, + default=0, + help='Priority value for this request' + ) + 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() + + with open(self.args.file) as f: + description = json.load(f) + + transient_qos = QoSProfile( + history=History.KEEP_LAST, + depth=1, + reliability=Reliability.RELIABLE, + durability=Durability.TRANSIENT_LOCAL) + self.pub = self.create_publisher( + 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.set_parameters([param]) + + # Construct task + msg = ApiRequest() + msg.request_id = "task_" + 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 + else: + self.get_logger().info("Using '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 + # todo(YV): Fill priority after schema is added + + # Define task request category + request["category"] = self.args.category + + # Define task request description + request["description"] = description + payload["request"] = request + msg.json_msg = json.dumps(payload) + + def receive_response(response_msg: ApiResponse): + if response_msg.request_id == msg.request_id: + self.response.set_result(json.loads(response_msg.json_msg)) + + transient_qos.depth = 10 + self.sub = self.create_subscription( + ApiResponse, 'task_api_responses', receive_response, transient_qos + ) + + print(f"Json msg payload: \n{json.dumps(payload, indent=2)}") + self.pub.publish(msg) + + +############################################################################### + + +def main(argv=sys.argv): + rclpy.init(args=sys.argv) + args_without_ros = rclpy.utilities.remove_ros_args(sys.argv) + + task_requester = TaskRequester(args_without_ros) + rclpy.spin_until_future_complete( + task_requester, task_requester.response, timeout_sec=5.0) + if task_requester.response.done(): + print(f'Got response:\n{task_requester.response.result()}') + else: + print('Did not get a response') + rclpy.shutdown() + + +if __name__ == '__main__': + main(sys.argv) + diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py index 0b1b95bb..82de7267 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py @@ -57,13 +57,13 @@ def __init__(self, argv=sys.argv): '-F', '--fleet', type=str, - help='Fleet name, should define tgt with robot', + help='Fleet name, should define together with robot', ) parser.add_argument( '-R', '--robot', type=str, - help='Robot name, should define tgt with fleet', + help='Robot name, should define together with fleet', ) parser.add_argument( '-st', From b3a06677f0edf7c97444e36fb5a2277c92eac773 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 27 Feb 2024 04:53:54 +0000 Subject: [PATCH 2/4] Add entrypoint for dispatch_json Signed-off-by: Michael X. Grey --- rmf_demos_tasks/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_demos_tasks/setup.py b/rmf_demos_tasks/setup.py index ec2c0666..a3725935 100644 --- a/rmf_demos_tasks/setup.py +++ b/rmf_demos_tasks/setup.py @@ -36,6 +36,7 @@ 'dispatch_teleop = rmf_demos_tasks.dispatch_teleop:main', 'mock_docker = rmf_demos_tasks.mock_docker:main', 'teleop_robot = rmf_demos_tasks.teleop_robot:main', + 'dispatch_json = rmf_demos_tasks.dispatch_json:main', ], }, ) From d818fdd08586910b6ea1a59715e71e5cf8ca8afa Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 27 Feb 2024 04:59:45 +0000 Subject: [PATCH 3/4] Fix style Signed-off-by: Michael X. Grey --- rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py index 4e4e5137..b981d44e 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py @@ -162,4 +162,3 @@ def main(argv=sys.argv): if __name__ == '__main__': main(sys.argv) - From 198cdf2e9d98ec50fd19f398ba20464ef893ac61 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 1 Mar 2024 07:31:40 +0000 Subject: [PATCH 4/4] Fix style Signed-off-by: Michael X. Grey --- .../rmf_demos_tasks/dispatch_json.py | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py b/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py index b981d44e..df6f4884 100644 --- a/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py +++ b/rmf_demos_tasks/rmf_demos_tasks/dispatch_json.py @@ -14,18 +14,18 @@ # 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 @@ -95,8 +95,8 @@ def __init__(self, argv=sys.argv): # 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 @@ -106,12 +106,12 @@ def __init__(self, argv=sys.argv): 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 = {} @@ -119,15 +119,15 @@ def __init__(self, argv=sys.argv): 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 + request['unix_millis_earliest_start_time'] = start_time # todo(YV): Fill priority after schema is added # Define task request category - request["category"] = self.args.category + request['category'] = self.args.category # Define task request description - request["description"] = description - payload["request"] = request + request['description'] = description + payload['request'] = request msg.json_msg = json.dumps(payload) def receive_response(response_msg: ApiResponse): @@ -139,7 +139,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)