feat: apply adapi routing (#75)

* feat: apply adapi routing

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

* feat: add auto route clear

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

* Update autoware_iv_external_api_adaptor/src/converter/routing.hpp

Co-authored-by: Kah Hooi Tan <41041286+tkhmy@users.noreply.github.com>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Kah Hooi Tan <41041286+tkhmy@users.noreply.github.com>

autoware_iv_external_api_adaptor/package.xml

@@ -11,6 +11,7 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_ad_api_specs</depend>
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>

autoware_iv_external_api_adaptor/src/converter/response_status.hpp

@@ -23,15 +23,15 @@
 namespace external_api::converter
 {
 
-namespace adapi = autoware_adapi_v1_msgs::msg;
-namespace tier4 = tier4_external_api_msgs::msg;
+using AdResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus;
+using T4ResponseStatus = tier4_external_api_msgs::msg::ResponseStatus;
 
-tier4::ResponseStatus convert(const adapi::ResponseStatus & in)
+inline T4ResponseStatus convert(const AdResponseStatus & ad)
 {
-  if (in.success) {
-    return tier4_api_utils::response_success(in.message);
+  if (ad.success) {
+    return tier4_api_utils::response_success(ad.message);
   } else {
-    return tier4_api_utils::response_error(in.message);
+    return tier4_api_utils::response_error(ad.message);
   }
 }
 

autoware_iv_external_api_adaptor/src/converter/routing.hpp

@@ -0,0 +1,85 @@
+// Copyright 2021 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 CONVERTER__ROUTING_HPP_
+#define CONVERTER__ROUTING_HPP_
+
+#include <autoware_adapi_v1_msgs/msg/route.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_route.hpp>
+#include <tier4_external_api_msgs/msg/route.hpp>
+#include <tier4_external_api_msgs/srv/set_route.hpp>
+
+namespace external_api::converter
+{
+
+using AdSetRoute = autoware_adapi_v1_msgs::srv::SetRoute::Request;
+using T4SetRoute = tier4_external_api_msgs::srv::SetRoute::Request;
+using AdRoute = autoware_adapi_v1_msgs::msg::Route;
+using T4Route = tier4_external_api_msgs::msg::Route;
+using AdSegment = autoware_adapi_v1_msgs::msg::RouteSegment;
+using T4Segment = tier4_external_api_msgs::msg::RouteSection;
+using AdPrimitive = autoware_adapi_v1_msgs::msg::RoutePrimitive;
+
+inline AdSegment convert(const T4Segment & t4)
+{
+  AdSegment ad;
+  for (const auto & id : t4.lane_ids) {
+    AdPrimitive primitive;
+    primitive.id = id;
+    primitive.type = "lane";
+    if (id == t4.preferred_lane_id) {
+      ad.preferred = primitive;
+    } else {
+      ad.alternatives.push_back(primitive);
+    }
+  }
+  return ad;
+}
+
+inline T4Segment convert(const AdSegment & ad)
+{
+  T4Segment t4;
+  t4.preferred_lane_id = ad.preferred.id;
+  t4.lane_ids.push_back(ad.preferred.id);
+  for (const auto & primitive : ad.alternatives) {
+    t4.lane_ids.push_back(primitive.id);
+  }
+  return t4;
+}
+
+inline AdSetRoute convert(const T4SetRoute & t4)
+{
+  AdSetRoute ad;
+  ad.header = t4.route.goal_pose.header;
+  ad.goal = t4.route.goal_pose.pose;
+  for (const auto & section : t4.route.route_sections) {
+    ad.segments.push_back(convert(section));
+  }
+  return ad;
+}
+
+inline T4Route convert(const AdRoute & ad)
+{
+  T4Route t4;
+  t4.goal_pose.header = ad.header;
+  t4.goal_pose.pose = ad.data.front().goal;
+  for (const auto & segment : ad.data.front().segments) {
+    t4.route_sections.push_back(convert(segment));
+  }
+  return t4;
+}
+
+}  // namespace external_api::converter
+
+#endif  // CONVERTER__ROUTING_HPP_

autoware_iv_external_api_adaptor/src/route.cpp

@@ -14,6 +14,9 @@
 
 #include "route.hpp"
 
+#include "converter/response_status.hpp"
+#include "converter/routing.hpp"
+
 #include <memory>
 
 namespace external_api
@@ -23,71 +26,69 @@ Route::Route(const rclcpp::NodeOptions & options) : Node("external_api_route", o
 {
   using std::placeholders::_1;
   using std::placeholders::_2;
-  tier4_api_utils::ServiceProxyNodeInterface proxy(this);
-
-  group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  srv_set_route_ = proxy.create_service<tier4_external_api_msgs::srv::SetRoute>(
-    "/api/external/set/route", std::bind(&Route::setRoute, this, _1, _2),
-    rmw_qos_profile_services_default, group_);
-  srv_clear_route_ = proxy.create_service<tier4_external_api_msgs::srv::ClearRoute>(
-    "/api/external/set/clear_route", std::bind(&Route::clearRoute, this, _1, _2),
-    rmw_qos_profile_services_default, group_);
-
-  cli_set_route_ =
-    proxy.create_client<tier4_external_api_msgs::srv::SetRoute>("/api/autoware/set/route");
-  cli_clear_route_ =
-    proxy.create_client<tier4_external_api_msgs::srv::ClearRoute>("/api/autoware/set/clear_route");
 
-  pub_get_route_ = create_publisher<tier4_external_api_msgs::msg::Route>(
-    "/api/external/get/route", rclcpp::QoS(1).transient_local());
-  sub_get_route_ = create_subscription<tier4_external_api_msgs::msg::Route>(
-    "/api/autoware/get/route", rclcpp::QoS(1).transient_local(),
-    std::bind(&Route::onRoute, this, _1));
-  sub_autoware_state_ = create_subscription<autoware_auto_system_msgs::msg::AutowareState>(
-    "/autoware/state", rclcpp::QoS(1), std::bind(&Route::onAutowareState, this, _1));
+  // external
+  {
+    tier4_api_utils::ServiceProxyNodeInterface proxy(this);
+    group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    srv_set_route_ = proxy.create_service<tier4_external_api_msgs::srv::SetRoute>(
+      "/api/external/set/route", std::bind(&Route::setRoute, this, _1, _2),
+      rmw_qos_profile_services_default, group_);
+    srv_clear_route_ = proxy.create_service<tier4_external_api_msgs::srv::ClearRoute>(
+      "/api/external/set/clear_route", std::bind(&Route::clearRoute, this, _1, _2),
+      rmw_qos_profile_services_default, group_);
+    pub_get_route_ = create_publisher<tier4_external_api_msgs::msg::Route>(
+      "/api/external/get/route", rclcpp::QoS(1).transient_local());
+  }
 
-  waiting_for_route_ = false;
+  // adapi
+  {
+    const auto adaptor = component_interface_utils::NodeAdaptor(this);
+    adaptor.init_cli(cli_clear_route_);
+    adaptor.init_cli(cli_set_route_);
+    adaptor.init_sub(sub_get_route_, this, &Route::onRoute);
+  }
 }
 
 void Route::setRoute(
   const tier4_external_api_msgs::srv::SetRoute::Request::SharedPtr request,
   const tier4_external_api_msgs::srv::SetRoute::Response::SharedPtr response)
 {
-  if (!waiting_for_route_) {
-    response->status = tier4_api_utils::response_error("It is not ready to set route.");
-    return;
+  // clear to overwrite
+  {
+    const auto req = std::make_shared<tier4_external_api_msgs::srv::ClearRoute::Request>();
+    const auto res = std::make_shared<tier4_external_api_msgs::srv::ClearRoute::Response>();
+    clearRoute(req, res);
   }
 
-  auto [status, resp] = cli_set_route_->call(request);
-  if (!tier4_api_utils::is_success(status)) {
-    response->status = status;
-    return;
+  try {
+    const auto req = std::make_shared<autoware_ad_api::routing::SetRoute::Service::Request>();
+    *req = converter::convert(*request);
+    const auto res = cli_set_route_->call(req);
+    response->status = converter::convert(res->status);
+  } catch (const component_interface_utils::ServiceException & error) {
+    response->status = tier4_api_utils::response_error(error.what());
   }
-  response->status = resp->status;
 }
 
 void Route::clearRoute(
-  const tier4_external_api_msgs::srv::ClearRoute::Request::SharedPtr request,
+  const tier4_external_api_msgs::srv::ClearRoute::Request::SharedPtr,
   const tier4_external_api_msgs::srv::ClearRoute::Response::SharedPtr response)
 {
-  // TODO(Takagi, Isamu): add a check after changing the state transition
-  auto [status, resp] = cli_clear_route_->call(request);
-  if (!tier4_api_utils::is_success(status)) {
-    response->status = status;
-    return;
+  try {
+    const auto req = std::make_shared<autoware_ad_api::routing::ClearRoute::Service::Request>();
+    const auto res = cli_clear_route_->call(req);
+    response->status = converter::convert(res->status);
+  } catch (const component_interface_utils::ServiceException & error) {
+    response->status = tier4_api_utils::response_error(error.what());
   }
-  response->status = resp->status;
 }
 
-void Route::onRoute(const tier4_external_api_msgs::msg::Route::ConstSharedPtr message)
+void Route::onRoute(const autoware_ad_api::routing::Route::Message::ConstSharedPtr message)
 {
-  pub_get_route_->publish(*message);
-}
-
-void Route::onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::SharedPtr message)
-{
-  using autoware_auto_system_msgs::msg::AutowareState;
-  waiting_for_route_ = (message->state == AutowareState::WAITING_FOR_ROUTE);
+  if (!message->data.empty()) {
+    pub_get_route_->publish(converter::convert(*message));
+  }
 }
 
 }  // namespace external_api

autoware_iv_external_api_adaptor/src/route.hpp

@@ -15,6 +15,8 @@
 #ifndef ROUTE_HPP_
 #define ROUTE_HPP_
 
+#include "autoware_ad_api_specs/routing.hpp"
+#include "component_interface_utils/rclcpp.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_api_utils/tier4_api_utils.hpp"
 
@@ -40,15 +42,13 @@ class Route : public rclcpp::Node
   // ros interface
   rclcpp::CallbackGroup::SharedPtr group_;
   tier4_api_utils::Service<SetRoute>::SharedPtr srv_set_route_;
-  tier4_api_utils::Client<SetRoute>::SharedPtr cli_set_route_;
   tier4_api_utils::Service<ClearRoute>::SharedPtr srv_clear_route_;
-  tier4_api_utils::Client<ClearRoute>::SharedPtr cli_clear_route_;
   rclcpp::Publisher<RouteMsg>::SharedPtr pub_get_route_;
-  rclcpp::Subscription<RouteMsg>::SharedPtr sub_get_route_;
-  rclcpp::Subscription<AutowareState>::SharedPtr sub_autoware_state_;
-
-  // class state
-  bool waiting_for_route_;
+  component_interface_utils::Client<autoware_ad_api::routing::ClearRoute>::SharedPtr
+    cli_clear_route_;
+  component_interface_utils::Client<autoware_ad_api::routing::SetRoute>::SharedPtr cli_set_route_;
+  component_interface_utils::Subscription<autoware_ad_api::routing::Route>::SharedPtr
+    sub_get_route_;
 
   // ros callback
   void setRoute(
@@ -57,8 +57,7 @@ class Route : public rclcpp::Node
   void clearRoute(
     const tier4_external_api_msgs::srv::ClearRoute::Request::SharedPtr request,
     const tier4_external_api_msgs::srv::ClearRoute::Response::SharedPtr response);
-  void onRoute(const tier4_external_api_msgs::msg::Route::ConstSharedPtr message);
-  void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::SharedPtr message);
+  void onRoute(const autoware_ad_api::routing::Route::Message::ConstSharedPtr message);
 };
 
 }  // namespace external_api

autoware_iv_internal_api_adaptor/CMakeLists.txt

@@ -7,14 +7,12 @@ autoware_package()
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/iv_msgs.cpp
   src/operator.cpp
-  src/route.cpp
   src/velocity.cpp
 )
 
 rclcpp_components_register_nodes(${PROJECT_NAME}
   "internal_api::IVMsgs"
   "internal_api::Operator"
-  "internal_api::Route"
   "internal_api::Velocity"
 )