diff --git a/autoware_iv_external_api_adaptor/package.xml b/autoware_iv_external_api_adaptor/package.xml
index 54fa466..194493a 100644
--- a/autoware_iv_external_api_adaptor/package.xml
+++ b/autoware_iv_external_api_adaptor/package.xml
@@ -11,6 +11,7 @@
autoware_cmake
+ autoware_ad_api_specs
autoware_auto_control_msgs
autoware_auto_system_msgs
autoware_auto_vehicle_msgs
diff --git a/autoware_iv_external_api_adaptor/src/converter/response_status.hpp b/autoware_iv_external_api_adaptor/src/converter/response_status.hpp
index 9910ef7..4ec9672 100644
--- a/autoware_iv_external_api_adaptor/src/converter/response_status.hpp
+++ b/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);
}
}
diff --git a/autoware_iv_external_api_adaptor/src/converter/routing.hpp b/autoware_iv_external_api_adaptor/src/converter/routing.hpp
new file mode 100644
index 0000000..9e40cb3
--- /dev/null
+++ b/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
+#include
+#include
+#include
+
+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_
diff --git a/autoware_iv_external_api_adaptor/src/route.cpp b/autoware_iv_external_api_adaptor/src/route.cpp
index 5df424d..5f06e48 100644
--- a/autoware_iv_external_api_adaptor/src/route.cpp
+++ b/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
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(
- "/api/external/set/route", std::bind(&Route::setRoute, this, _1, _2),
- rmw_qos_profile_services_default, group_);
- srv_clear_route_ = proxy.create_service(
- "/api/external/set/clear_route", std::bind(&Route::clearRoute, this, _1, _2),
- rmw_qos_profile_services_default, group_);
-
- cli_set_route_ =
- proxy.create_client("/api/autoware/set/route");
- cli_clear_route_ =
- proxy.create_client("/api/autoware/set/clear_route");
- pub_get_route_ = create_publisher(
- "/api/external/get/route", rclcpp::QoS(1).transient_local());
- sub_get_route_ = create_subscription(
- "/api/autoware/get/route", rclcpp::QoS(1).transient_local(),
- std::bind(&Route::onRoute, this, _1));
- sub_autoware_state_ = create_subscription(
- "/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(
+ "/api/external/set/route", std::bind(&Route::setRoute, this, _1, _2),
+ rmw_qos_profile_services_default, group_);
+ srv_clear_route_ = proxy.create_service(
+ "/api/external/set/clear_route", std::bind(&Route::clearRoute, this, _1, _2),
+ rmw_qos_profile_services_default, group_);
+ pub_get_route_ = create_publisher(
+ "/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();
+ const auto res = std::make_shared();
+ 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();
+ *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();
+ 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
diff --git a/autoware_iv_external_api_adaptor/src/route.hpp b/autoware_iv_external_api_adaptor/src/route.hpp
index 2e6b8d9..817bc6f 100644
--- a/autoware_iv_external_api_adaptor/src/route.hpp
+++ b/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::SharedPtr srv_set_route_;
- tier4_api_utils::Client::SharedPtr cli_set_route_;
tier4_api_utils::Service::SharedPtr srv_clear_route_;
- tier4_api_utils::Client::SharedPtr cli_clear_route_;
rclcpp::Publisher::SharedPtr pub_get_route_;
- rclcpp::Subscription::SharedPtr sub_get_route_;
- rclcpp::Subscription::SharedPtr sub_autoware_state_;
-
- // class state
- bool waiting_for_route_;
+ component_interface_utils::Client::SharedPtr
+ cli_clear_route_;
+ component_interface_utils::Client::SharedPtr cli_set_route_;
+ component_interface_utils::Subscription::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
diff --git a/autoware_iv_internal_api_adaptor/CMakeLists.txt b/autoware_iv_internal_api_adaptor/CMakeLists.txt
index 66de477..08afb9d 100644
--- a/autoware_iv_internal_api_adaptor/CMakeLists.txt
+++ b/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"
)
diff --git a/autoware_iv_internal_api_adaptor/src/route.cpp b/autoware_iv_internal_api_adaptor/src/route.cpp
deleted file mode 100644
index 430d10b..0000000
--- a/autoware_iv_internal_api_adaptor/src/route.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-// 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.
-
-#include "route.hpp"
-
-#include
-#include
-#include
-#include
-#include
-
-#include
-
-namespace
-{
-auto convertRoute(const tier4_external_api_msgs::msg::Route & route)
-{
- // there is no input for start_pose
- autoware_auto_planning_msgs::msg::HADMapRoute msg;
- msg.header = route.goal_pose.header;
- msg.goal_pose = route.goal_pose.pose;
- for (const auto & section : route.route_sections) {
- autoware_auto_mapping_msgs::msg::HADMapSegment segment;
- segment.preferred_primitive_id = section.preferred_lane_id;
- for (const auto & lane_id : section.lane_ids) {
- autoware_auto_mapping_msgs::msg::MapPrimitive primitive;
- primitive.id = lane_id;
- segment.primitives.push_back(primitive);
- }
- msg.segments.push_back(segment);
- }
- return msg;
-}
-
-auto convertRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route)
-{
- // there is no input for continued_lane_ids
- tier4_external_api_msgs::msg::Route msg;
- msg.goal_pose.header = route.header;
- msg.goal_pose.pose = route.goal_pose;
- for (const auto & segment : route.segments) {
- tier4_external_api_msgs::msg::RouteSection section;
- section.preferred_lane_id = segment.preferred_primitive_id;
- for (const auto & primitive : segment.primitives) {
- section.lane_ids.push_back(primitive.id);
- }
- msg.route_sections.push_back(section);
- }
- return msg;
-}
-
-} // namespace
-
-namespace internal_api
-{
-Route::Route(const rclcpp::NodeOptions & options) : Node("external_api_route", options)
-{
- using std::placeholders::_1;
- using std::placeholders::_2;
- tier4_api_utils::ServiceProxyNodeInterface proxy(this);
-
- group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
- srv_clear_route_ = proxy.create_service(
- "/api/autoware/set/clear_route", std::bind(&Route::clearRoute, this, _1, _2),
- rmw_qos_profile_services_default, group_);
- srv_set_route_ = proxy.create_service(
- "/api/autoware/set/route", std::bind(&Route::setRoute, this, _1, _2),
- rmw_qos_profile_services_default, group_);
- srv_set_goal_ = proxy.create_service(
- "/api/autoware/set/goal", std::bind(&Route::setGoal, this, _1, _2),
- rmw_qos_profile_services_default, group_);
- srv_set_checkpoint_ = proxy.create_service(
- "/api/autoware/set/checkpoint", std::bind(&Route::setCheckpoint, this, _1, _2),
- rmw_qos_profile_services_default, group_);
-
- pub_get_route_ = create_publisher(
- "/api/autoware/get/route", rclcpp::QoS(1).transient_local());
-
- cli_clear_route_ = proxy.create_client("/autoware/reset_route");
- sub_planning_route_ = create_subscription(
- "/planning/mission_planning/route", rclcpp::QoS(1).transient_local(),
- std::bind(&Route::onRoute, this, _1));
- pub_planning_route_ = create_publisher(
- "/planning/mission_planning/route", rclcpp::QoS(1).transient_local());
- pub_planning_goal_ = create_publisher(
- "/planning/mission_planning/goal", rclcpp::QoS(1));
- pub_planning_checkpoint_ = create_publisher(
- "/planning/mission_planning/checkpoint", rclcpp::QoS(1));
-}
-
-void Route::clearRoute(
- const tier4_external_api_msgs::srv::ClearRoute::Request::SharedPtr,
- const tier4_external_api_msgs::srv::ClearRoute::Response::SharedPtr response)
-{
- auto req = std::make_shared();
- auto [status, resp] = cli_clear_route_->call(req);
- if (!tier4_api_utils::is_success(status)) {
- response->status = status;
- return;
- }
- if (resp->success) {
- response->status = tier4_api_utils::response_success(resp->message);
- } else {
- response->status = tier4_api_utils::response_error(resp->message);
- }
-}
-
-void Route::setRoute(
- const tier4_external_api_msgs::srv::SetRoute::Request::SharedPtr request,
- const tier4_external_api_msgs::srv::SetRoute::Response::SharedPtr response)
-{
- pub_planning_route_->publish(convertRoute(request->route));
- response->status = tier4_api_utils::response_success();
-}
-
-void Route::setGoal(
- const tier4_external_api_msgs::srv::SetPose::Request::SharedPtr request,
- const tier4_external_api_msgs::srv::SetPose::Response::SharedPtr response)
-{
- pub_planning_goal_->publish(request->pose);
- response->status = tier4_api_utils::response_success();
-}
-
-void Route::setCheckpoint(
- const tier4_external_api_msgs::srv::SetPose::Request::SharedPtr request,
- const tier4_external_api_msgs::srv::SetPose::Response::SharedPtr response)
-{
- pub_planning_checkpoint_->publish(request->pose);
- response->status = tier4_api_utils::response_success();
-}
-
-void Route::onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr message)
-{
- pub_get_route_->publish(convertRoute(*message));
-}
-
-} // namespace internal_api
-
-#include
-RCLCPP_COMPONENTS_REGISTER_NODE(internal_api::Route)
diff --git a/autoware_iv_internal_api_adaptor/src/route.hpp b/autoware_iv_internal_api_adaptor/src/route.hpp
deleted file mode 100644
index 4a98bac..0000000
--- a/autoware_iv_internal_api_adaptor/src/route.hpp
+++ /dev/null
@@ -1,74 +0,0 @@
-// 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 ROUTE_HPP_
-#define ROUTE_HPP_
-
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-#include
-
-namespace internal_api
-{
-class Route : public rclcpp::Node
-{
-public:
- explicit Route(const rclcpp::NodeOptions & options);
-
-private:
- using ClearRoute = tier4_external_api_msgs::srv::ClearRoute;
- using SetRoute = tier4_external_api_msgs::srv::SetRoute;
- using SetPose = tier4_external_api_msgs::srv::SetPose;
- using HADMapRoute = autoware_auto_planning_msgs::msg::HADMapRoute;
-
- // ros interface
- rclcpp::CallbackGroup::SharedPtr group_;
- tier4_api_utils::Service::SharedPtr srv_clear_route_;
- tier4_api_utils::Service::SharedPtr srv_set_route_;
- tier4_api_utils::Service::SharedPtr srv_set_goal_;
- tier4_api_utils::Service::SharedPtr srv_set_checkpoint_;
- tier4_api_utils::Client::SharedPtr cli_clear_route_;
- rclcpp::Subscription::SharedPtr sub_planning_route_;
- rclcpp::Publisher::SharedPtr pub_planning_route_;
- rclcpp::Publisher::SharedPtr pub_planning_goal_;
- rclcpp::Publisher::SharedPtr pub_planning_checkpoint_;
- rclcpp::Publisher::SharedPtr pub_get_route_;
-
- // ros callback
- void clearRoute(
- const tier4_external_api_msgs::srv::ClearRoute::Request::SharedPtr request,
- const tier4_external_api_msgs::srv::ClearRoute::Response::SharedPtr response);
- void setRoute(
- const tier4_external_api_msgs::srv::SetRoute::Request::SharedPtr request,
- const tier4_external_api_msgs::srv::SetRoute::Response::SharedPtr response);
- void setGoal(
- const tier4_external_api_msgs::srv::SetPose::Request::SharedPtr request,
- const tier4_external_api_msgs::srv::SetPose::Response::SharedPtr response);
- void setCheckpoint(
- const tier4_external_api_msgs::srv::SetPose::Request::SharedPtr request,
- const tier4_external_api_msgs::srv::SetPose::Response::SharedPtr response);
-
- void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr message);
-};
-
-} // namespace internal_api
-
-#endif // ROUTE_HPP_