feat(default_ad_api): add routing api (#1494)

* feat(default_ad_api): add routing api

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: build error

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* docs: add readme

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: change topic namespace

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: function name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: remove debug code

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: copyright

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: adaptor name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: remove macro

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: add launch option for default ad api

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: component interface namespace

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: build error

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: remove start pose

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(autoware_ad_api_msgs): define routing interface

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: rename route body message

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: remove create node macro

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: adaptor package

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: helper node

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: error handling

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml

@@ -2,10 +2,16 @@
   <arg name="respawn_rosbridge" default="true"/>
   <arg name="launch_image_base64_converter" default="false"/>
   <arg name="launch_path_distance_calculator" default="false"/>
+  <arg name="launch_default_ad_api" default="true"/>
   <!-- external api adaptor -->
   <arg name="launch_calibration_status_api" default="false"/>
   <arg name="launch_start_api" default="true"/>
 
+  <!-- AD API -->
+  <group if="$(var launch_default_ad_api)">
+    <include file="$(find-pkg-share default_ad_api)/launch/default_ad_api.launch.py"/>
+  </group>
+
   <!-- awapi (deprecated) -->
   <group>
     <include file="$(find-pkg-share awapi_awiv_adapter)/launch/awapi_awiv_adapter.launch.xml"/>

system/default_ad_api/CMakeLists.txt

@@ -6,9 +6,13 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/interface.cpp
+  src/routing.cpp
 )
 
-rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::InterfaceNode")
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  "default_ad_api::InterfaceNode"
+  "default_ad_api::RoutingNode"
+)
 
 if(BUILD_TESTING)
   add_launch_test(test/main.test.py)

system/default_ad_api/launch/default_ad_api.launch.py

@@ -31,6 +31,7 @@ def _create_api_node(node_name, class_name, **kwargs):
 def generate_launch_description():
     components = [
         _create_api_node("interface", "InterfaceNode"),
+        _create_api_node("routing", "RoutingNode"),
     ]
     container = ComposableNodeContainer(
         namespace="default_ad_api",

system/default_ad_api/package.xml

@@ -13,6 +13,7 @@
 
   <depend>autoware_ad_api_msgs</depend>
   <depend>autoware_ad_api_specs</depend>
+  <depend>component_interface_specs</depend>
   <depend>component_interface_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

system/default_ad_api/src/routing.cpp

@@ -0,0 +1,34 @@
+// Copyright 2022 TIER IV, 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.
+
+#include "routing.hpp"
+
+namespace default_ad_api
+{
+
+RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", options)
+{
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  group_srv_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  adaptor.relay_message(pub_route_state_, sub_route_state_);
+  adaptor.relay_message(pub_route_, sub_route_);
+  adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_srv_);
+  adaptor.relay_service(cli_set_route_, srv_set_route_, group_srv_);
+  adaptor.relay_service(cli_clear_route_, srv_clear_route_, group_srv_);
+}
+
+}  // namespace default_ad_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::RoutingNode)

system/default_ad_api/src/routing.hpp

@@ -0,0 +1,49 @@
+// Copyright 2022 TIER IV, 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.
+
+#ifndef ROUTING_HPP_
+#define ROUTING_HPP_
+
+#include <autoware_ad_api_specs/routing.hpp>
+#include <component_interface_specs/planning.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+// This file should be included after messages.
+#include "utils/types.hpp"
+
+namespace default_ad_api
+{
+
+class RoutingNode : public rclcpp::Node
+{
+public:
+  explicit RoutingNode(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::CallbackGroup::SharedPtr group_srv_;
+  Pub<autoware_ad_api::routing::RouteState> pub_route_state_;
+  Pub<autoware_ad_api::routing::Route> pub_route_;
+  Srv<autoware_ad_api::routing::SetRoutePoints> srv_set_route_points_;
+  Srv<autoware_ad_api::routing::SetRoute> srv_set_route_;
+  Srv<autoware_ad_api::routing::ClearRoute> srv_clear_route_;
+  Sub<planning_interface::RouteState> sub_route_state_;
+  Sub<planning_interface::Route> sub_route_;
+  Cli<planning_interface::SetRoutePoints> cli_set_route_points_;
+  Cli<planning_interface::SetRoute> cli_set_route_;
+  Cli<planning_interface::ClearRoute> cli_clear_route_;
+};
+
+}  // namespace default_ad_api
+
+#endif  // ROUTING_HPP_

system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.14)
+project(ad_api_adaptors)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(routing_adaptor
+  src/routing_adaptor.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE launch)

system/default_ad_api_helpers/ad_api_adaptors/README.md

@@ -0,0 +1,15 @@
+# ad_api_adaptors
+
+## routing_adaptor
+
+This node makes it easy to use the routing AD API from RViz.
+When a goal pose topic is received, reset the waypoints and call the API.
+When a waypoint pose topic is received, append it to the end of the waypoints to call the API.
+The clear API is called automatically before setting the route.
+
+| Interface    | Local Name       | Global Name                           | Description                 |
+| ------------ | ---------------- | ------------------------------------- | --------------------------- |
+| Subscription | ~/input/goal     | /planning/mission_planning/goal       | The goal pose of route.     |
+| Subscription | ~/input/waypoint | /planning/mission_planning/checkpoint | The waypoint pose of route. |
+| Client       | -                | /api/routing/clear_route              | The route clear API.        |
+| Client       | -                | /api/routing/set_route_points         | The route points set API.   |

system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="ad_api_adaptors" exec="initial_pose_adaptor" name="initial_pose_adaptor">
+    <remap from="~/input/goal" to="/planning/mission_planning/goal"/>
+    <remap from="~/input/waypoint" to="/planning/mission_planning/checkpoint"/>
+  </node>
+</launch>

system/default_ad_api_helpers/ad_api_adaptors/package.xml

@@ -0,0 +1,25 @@
+<?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>ad_api_adaptors</name>
+  <version>0.1.0</version>
+  <description>The ad_api_adaptors package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_ad_api_msgs</depend>
+  <depend>autoware_ad_api_specs</depend>
+  <depend>component_interface_utils</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp

@@ -0,0 +1,68 @@
+// Copyright 2022 TIER IV, 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.
+
+#include "routing_adaptor.hpp"
+
+#include <memory>
+
+namespace ad_api_adaptors
+{
+
+RoutingAdaptor::RoutingAdaptor() : Node("routing_adaptor")
+{
+  sub_goal_ = create_subscription<PoseStamped>(
+    "~/input/goal", 5, std::bind(&RoutingAdaptor::on_goal, this, std::placeholders::_1));
+  sub_waypoint_ = create_subscription<PoseStamped>(
+    "~/input/waypoint", 5, std::bind(&RoutingAdaptor::on_waypoint, this, std::placeholders::_1));
+
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_cli(cli_route_);
+  adaptor.init_cli(cli_clear_);
+  route_points_ = std::make_shared<SetRoutePoints::Service::Request>();
+}
+
+void RoutingAdaptor::on_goal(const PoseStamped::ConstSharedPtr pose)
+{
+  route_points_->header = pose->header;
+  route_points_->goal = pose->pose;
+  route_points_->waypoints.clear();
+
+  cli_clear_->async_send_request(std::make_shared<ClearRoute::Service::Request>());
+  cli_route_->async_send_request(route_points_);
+}
+
+void RoutingAdaptor::on_waypoint(const PoseStamped::ConstSharedPtr pose)
+{
+  if (route_points_->header.frame_id != pose->header.frame_id) {
+    RCLCPP_ERROR_STREAM(get_logger(), "The waypoint frame does not match the goal.");
+    return;
+  }
+  route_points_->waypoints.push_back(pose->pose);
+
+  cli_clear_->async_send_request(std::make_shared<ClearRoute::Service::Request>());
+  cli_route_->async_send_request(route_points_);
+}
+
+}  // namespace ad_api_adaptors
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::executors::SingleThreadedExecutor executor;
+  auto node = std::make_shared<ad_api_adaptors::RoutingAdaptor>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
+  rclcpp::shutdown();
+}

system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp

@@ -0,0 +1,49 @@
+// Copyright 2022 TIER IV, 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.
+
+#ifndef ROUTING_ADAPTOR_HPP_
+#define ROUTING_ADAPTOR_HPP_
+
+#include <autoware_ad_api_specs/routing.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <string>
+
+namespace ad_api_adaptors
+{
+
+class RoutingAdaptor : public rclcpp::Node
+{
+public:
+  RoutingAdaptor();
+
+private:
+  using PoseStamped = geometry_msgs::msg::PoseStamped;
+  using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints;
+  using ClearRoute = autoware_ad_api::routing::ClearRoute;
+  SetRoutePoints::Service::Request::SharedPtr route_points_;
+  component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
+  component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_goal_;
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_waypoint_;
+  void on_goal(const PoseStamped::ConstSharedPtr pose);
+  void on_waypoint(const PoseStamped::ConstSharedPtr pose);
+};
+
+}  // namespace ad_api_adaptors
+
+#endif  // ROUTING_ADAPTOR_HPP_