Add example showing how to go to nearest spot

This pr depends on:
* open-rmf/rmf_task#101
* open-rmf/rmf_ros2#308

You can ask a robot to go to its nearest spot using the newly added
script.

Signed-off-by: Arjo Chakravarty <[email protected]>

rmf_demos_tasks/README.md

@@ -105,6 +105,15 @@ The new task system allows users to construct and submit their own tasks in a mo
   ros2 run rmf_demos_tasks  dispatch_go_to_place -p lounge -o 105 --use_sim_time
   ```
 
+  **dispatch_go_to_nearest_place**
+  Similar to `dispatch_go_to_place`. Sends a robot to the nearest waypoint from a list of provided waypoints.
+
+  Example in office world:
+  ```
+  ros2 run rmf_demos_tasks dispatch_go_to_nearest_place -F tinyRobot -R tinyRobot1 -p supplies,pantry --use_sim_time
+  ```
+  If the robot is nearer supplies it will go to supplies, otherwise it goes to the pantry.
+
 ## Quality Declaration
 
 This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](./QUALITY_DECLARATION.md) for more details.

rmf_demos_tasks/rmf_demos_tasks/dispatch_go_to_nearest_place.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python3
+
+# Copyright 2022 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import sys
+import uuid
+import argparse
+import json
+import asyncio
+import math
+
+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 QoSReliabilityPolicy as Reliability
+
+from rmf_task_msgs.msg import ApiRequest, ApiResponse
+
+def list_of_strings(arg):
+    return arg.split(',')
+###############################################################################
+
+class TaskRequester(Node):
+
+    def __init__(self, argv=sys.argv):
+        super().__init__('task_requester')
+        parser = argparse.ArgumentParser()
+        parser.add_argument('-F', '--fleet', type=str, help='Fleet name')
+        parser.add_argument('-R', '--robot', type=str, help='Robot name')
+        parser.add_argument(
+            '-p', '--places', required=True, type=list_of_strings,
+            help='Places to go to seperated by comma'
+        )
+        parser.add_argument('-st', '--start_time',
+                            help='Start time from now in secs, default: 0',
+                            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()
+
+        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 = "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
+        else:
+            self.get_logger().info("Using '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)
+
+        # Define task request description
+        go_to_description = {'one_of': [ {"waypoint": wp} for wp in self.args.places],
+                             'constraints' : [{
+                                "category": "same_floor",
+                                "description": ""
+        }]}
+
+        go_to_activity = {
+            'category': 'go_to_place',
+            'description': go_to_description
+        }
+
+        rmf_task_request = {
+            'category': 'compose',
+            'description': {
+                'category': 'go_to_place',
+                'phases': [{'activity': go_to_activity}]
+            },
+            'unix_millis_earliest_start_time': start_time
+        }
+
+        payload["request"] = rmf_task_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))
+
+        self.sub = self.create_subscription(
+            ApiResponse, 'task_api_responses', receive_response, 10
+        )
+
+        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)

rmf_demos_tasks/setup.py

@@ -31,6 +31,7 @@
           '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_go_to_nearest_place = rmf_demos_tasks.dispatch_go_to_nearest_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',