feat(route_client): add route_client (#67)

Signed-off-by: Fumiya Watanabe <[email protected]>

planning/autoware_route_client/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_route_client)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(std_msgs REQUIRED)
+
+ament_auto_package()
+
+install(PROGRAMS
+  scripts/route_client.py
+  DESTINATION lib/${PROJECT_NAME}
+)

planning/autoware_route_client/README.md

@@ -0,0 +1,48 @@
+# Route Client
+
+This package contains a tool to send request to set route.
+
+## Usage
+
+### Prepare a route file
+
+Prepare a YAML file containing route information.
+The file format is like following:
+
+```yaml
+goal:
+  position:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 0.0
+segments:
+  - preferred:
+      id: 0
+      type: lane
+    alternatives:
+      - id: 1
+        type: lane
+  - preferred:
+      id: 2
+      type: lane
+    alternatives: []
+  - preferred:
+      id: 3
+      type: lane
+    alternatives:
+      - id: 4
+        type: lane
+```
+
+### Send request to set route
+
+Execute following command.
+
+```bash
+ros2 run autoware_route_client route_client.py <path_to_yaml_file>
+```

planning/autoware_route_client/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_route_client</name>
+  <version>0.1.0</version>
+  <description>The autoware_route_client package</description>
+  <maintainer email="[email protected]">Fumiya Watanabe</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Fumiya Watanabe</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

planning/autoware_route_client/scripts/route_client.py

@@ -0,0 +1,107 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# Copyright 2024 TIER IV, Inc. All rights reserved.
+#
+# 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 argparse
+
+from autoware_adapi_v1_msgs.msg import RouteOption
+from autoware_adapi_v1_msgs.msg import RoutePrimitive
+from autoware_adapi_v1_msgs.msg import RouteSegment
+from autoware_adapi_v1_msgs.srv import SetRoute
+from geometry_msgs.msg import Pose
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Header
+import yaml
+
+
+class route_client(Node):
+    def __init__(self):
+        super().__init__("route_client")
+        self.cli = self.create_client(SetRoute, "/api/routing/change_route")
+
+        while not self.cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("service not available...")
+
+        self.req = SetRoute.Request()
+
+    def send_request(self, filepath):
+        self.req.header = Header()
+        self.req.header.frame_id = "map"
+        self.req.header.stamp = self.get_clock().now().to_msg()
+        self.req.option = RouteOption()
+        self.req.option.allow_goal_modification = True
+
+        with open(filepath) as file:
+            data = yaml.safe_load(file)
+            self.req.goal = self.set_goal(data["goal"])
+            self.req.segments = self.set_segments(data["segments"])
+            self.future = self.cli.call_async(self.req)
+
+    def set_goal(self, goal):
+        out = Pose()
+        out.position.x = goal["position"]["x"]
+        out.position.y = goal["position"]["y"]
+        out.position.z = goal["position"]["z"]
+        out.orientation.x = goal["orientation"]["x"]
+        out.orientation.y = goal["orientation"]["y"]
+        out.orientation.z = goal["orientation"]["z"]
+        out.orientation.w = goal["orientation"]["w"]
+
+        return out
+
+    def set_segments(self, segments):
+        out = []
+        for segment in segments:
+            out_segment = RouteSegment()
+            out_segment.preferred.id = segment["preferred"]["id"]
+            out_segment.preferred.type = segment["preferred"]["type"]
+            for alt in segment["alternatives"]:
+                out_alt = RoutePrimitive()
+                out_alt.id = alt["id"]
+                out_alt.type = alt["type"]
+                out_segment.alternatives.append(out_alt)
+
+            out.append(out_segment)
+        return out
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    parser = argparse.ArgumentParser(description="Send set route request.")
+    parser.add_argument("filepath", type=str, help="path to yaml file containing route information")
+    args = parser.parse_args()
+
+    client = route_client()
+    client.send_request(args.filepath)
+
+    while rclpy.ok():
+        rclpy.spin_once(client)
+        if client.future.done():
+            try:
+                client.get_logger().info("Service requested.")
+            except Exception as e:
+                client.get_logger().info("Error: %r" % (e,))
+            break
+
+    client.destroy_node()
+
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()