Support one_of for go_to_place (#200)

* Support one_of for go_to_place

Signed-off-by: Michael X. Grey <grey@openrobotics.org>

* Fix missing bracket from merge conflict resolution

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Format

Signed-off-by: Yadunund <yadunund@openrobotics.org>

* Add flake8 and pep257 tests and fix issues

Signed-off-by: Yadunund <yadunund@openrobotics.org>

---------

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
Signed-off-by: Yadunund <yadunund@openrobotics.org>
Co-authored-by: Yadunund <yadunund@openrobotics.org>

rmf_demos_tasks/rmf_demos_tasks/cancel_task.py

@@ -21,8 +21,6 @@
 
 import rclpy
 from rclpy.node import Node
-from rclpy.parameter import Parameter
-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
@@ -33,6 +31,7 @@
 
 
 class TaskRequester(Node):
+
     def __init__(self, argv=sys.argv):
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()
@@ -78,6 +77,7 @@ def main(argv=sys.argv):
     args_without_ros = rclpy.utilities.remove_ros_args(sys.argv)
 
     task_requester = TaskRequester(args_without_ros)
+    rclpy.spin_once(task_requester)
     rclpy.shutdown()
 
 

rmf_demos_tasks/rmf_demos_tasks/dispatch_action.py

@@ -23,7 +23,6 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-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
@@ -35,6 +34,7 @@
 
 
 class TaskRequester(Node):
+
     def __init__(self, argv=sys.argv):
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()

rmf_demos_tasks/rmf_demos_tasks/dispatch_clean.py

@@ -23,7 +23,6 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-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
@@ -35,6 +34,7 @@
 
 
 class TaskRequester(Node):
+
     def __init__(self, argv=sys.argv):
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()

rmf_demos_tasks/rmf_demos_tasks/dispatch_delivery.py

@@ -23,7 +23,6 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-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
@@ -35,6 +34,7 @@
 
 
 class TaskRequester(Node):
+
     def __init__(self, argv=sys.argv):
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()

rmf_demos_tasks/rmf_demos_tasks/dispatch_go_to_place.py

@@ -24,7 +24,6 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-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
@@ -36,13 +35,19 @@
 
 
 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', '--place', required=True, type=str, help='Place to go to'
+            '-p',
+            '--place',
+            required=True,
+            type=str,
+            help='Place to go to',
+            nargs='+',
         )
         parser.add_argument(
             '-o',
@@ -51,6 +56,16 @@ def __init__(self, argv=sys.argv):
             type=float,
             help='Orientation to face in degrees (optional)',
         )
+        parser.add_argument(
+            '-m',
+            '--prefer-same-map',
+            required=False,
+            action='store_true',
+            help=(
+                'When choosing between multiple destination options, prefer '
+                'an option on the same map as the starting location.'
+            ),
+        )
         parser.add_argument(
             '-st',
             '--start_time',
@@ -112,11 +127,21 @@ def __init__(self, argv=sys.argv):
         # 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
-            )
+        one_of = []
+        for place in self.args.place:
+            place_json = {'waypoint': place}
+            if self.args.orientation is not None:
+                place_json['orientation'] = (
+                    self.args.orientation * math.pi / 180.0
+                )
+            one_of.append(place_json)
+
+        go_to_description = {'one_of': one_of}
+
+        if self.args.prefer_same_map:
+            go_to_description['constraints'] = [
+                {'category': 'prefer_same_map'}
+            ]
 
         go_to_activity = {
             'category': 'go_to_place',

rmf_demos_tasks/rmf_demos_tasks/dispatch_loop.py

@@ -16,11 +16,8 @@
 
 import argparse
 import sys
-import time
-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
@@ -30,6 +27,7 @@
 
 
 class TaskRequester:
+
     def __init__(self, argv=sys.argv):
         parser = argparse.ArgumentParser()
         parser.add_argument(

rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py

@@ -34,6 +34,7 @@
 
 
 class TaskRequester(Node):
+
     def __init__(self, argv=sys.argv):
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()

rmf_demos_tasks/rmf_demos_tasks/dispatch_teleop.py

@@ -23,7 +23,6 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-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
@@ -35,6 +34,7 @@
 
 
 class TaskRequester(Node):
+
     def __init__(self, argv=sys.argv):
         super().__init__('task_requester')
         parser = argparse.ArgumentParser()

rmf_demos_tasks/rmf_demos_tasks/mock_docker.py

@@ -23,7 +23,6 @@
 from rclpy.qos import QoSHistoryPolicy as History
 from rclpy.qos import QoSProfile
 from rclpy.qos import QoSReliabilityPolicy as Reliability
-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
@@ -55,7 +54,8 @@ def close(l0: Location, l1: Location):
 
 class MockDocker(Node):
     """
-    The MockDocker has two objectices
+    The MockDocker has two objectives.
+
     1) Publish a DockSummary message with information on all the docking
     processes in a building. The fleet adapters rely on this message for
     task planning.
@@ -72,7 +72,7 @@ class MockDocker(Node):
 
     def __init__(self, config_yaml):
         super().__init__('mock_docker')
-        self.get_logger().info(f'Greetings, I am mock docker')
+        self.get_logger().info('Greetings, I am mock docker')
         self.config_yaml = config_yaml
         self.path_request_publisher = self.create_publisher(
             PathRequest, 'robot_path_requests', 1
@@ -136,7 +136,7 @@ def mode_request_cb(self, msg: ModeRequest):
 
         if not msg.parameters:
             self.get_logger().warn(
-                f'Missing docking name for docking request!'
+                'Missing docking name for docking request!'
             )
             return
 

rmf_demos_tasks/rmf_demos_tasks/request_lift.py

@@ -1,4 +1,17 @@
-import argparse
+# 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
 from time import sleep
 import uuid

rmf_demos_tasks/rmf_demos_tasks/request_loop.py

@@ -20,12 +20,11 @@
 import uuid
 
 import rclpy
-from rclpy.node import Node
-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')
@@ -76,9 +75,7 @@ def main(self):
 
         self.node.get_logger().info(
             'Loop request between {} and {}, submitted to {} robot '
-            'fleet'.format(
-                self.start_wp, self.finish_wp, self.robot_type
-            )
+            'fleet'.format(self.start_wp, self.finish_wp, self.robot_type)
         )
 
 

rmf_demos_tasks/rmf_demos_tasks/teleop_robot.py

@@ -25,14 +25,14 @@
 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 Location
 from rmf_fleet_msgs.msg import PathRequest
 
 ###############################################################################
 
 
 class Requester(Node):
+
     def __init__(self, argv=sys.argv):
         super().__init__('teleop_publisher')
         parser = argparse.ArgumentParser()
@@ -96,7 +96,7 @@ def main(argv=sys.argv):
     def trigger_timeout():
         timeout.set_result(True)
 
-    timer = requester.create_timer(5.0, trigger_timeout)
+    _ = requester.create_timer(5.0, trigger_timeout)
 
     rclpy.spin_until_future_complete(requester, timeout)
     rclpy.shutdown()

rmf_demos_tasks/test/test_copyright.py

@@ -0,0 +1,23 @@
+# 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

rmf_demos_tasks/test/test_flake8.py

@@ -0,0 +1,25 @@
+# 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, 'Found %d code style errors / warnings:\n' % len(
+        errors
+    ) + '\n'.join(errors)

rmf_demos_tasks/test/test_pep257.py

@@ -0,0 +1,23 @@
+# 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'