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_