diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index 914ca0d46e2d0..a98ac01548732 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -2,10 +2,16 @@ + + + + + + diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index ae4125cc822ff..e3f60904c24fa 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/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) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index e9542328ed986..7e11805870c5b 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/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", diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 36ffdefaa0482..9826f5af66f1c 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -13,6 +13,7 @@ autoware_ad_api_msgs autoware_ad_api_specs + component_interface_specs component_interface_utils rclcpp rclcpp_components diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp new file mode 100644 index 0000000000000..c09e483be1a28 --- /dev/null +++ b/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(default_ad_api::RoutingNode) diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp new file mode 100644 index 0000000000000..70be836c73643 --- /dev/null +++ b/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 +#include +#include + +// 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 pub_route_state_; + Pub pub_route_; + Srv srv_set_route_points_; + Srv srv_set_route_; + Srv srv_clear_route_; + Sub sub_route_state_; + Sub sub_route_; + Cli cli_set_route_points_; + Cli cli_set_route_; + Cli cli_clear_route_; +}; + +} // namespace default_ad_api + +#endif // ROUTING_HPP_ diff --git a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt b/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt new file mode 100644 index 0000000000000..65028d9ba5cc5 --- /dev/null +++ b/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) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md new file mode 100644 index 0000000000000..bb25903043bac --- /dev/null +++ b/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. | diff --git a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml new file mode 100644 index 0000000000000..976d0868fe20e --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml new file mode 100644 index 0000000000000..b98d230a4c3cf --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -0,0 +1,25 @@ + + + + ad_api_adaptors + 0.1.0 + The ad_api_adaptors package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_ad_api_msgs + autoware_ad_api_specs + component_interface_utils + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp new file mode 100644 index 0000000000000..4ea3a77a179b1 --- /dev/null +++ b/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 + +namespace ad_api_adaptors +{ + +RoutingAdaptor::RoutingAdaptor() : Node("routing_adaptor") +{ + sub_goal_ = create_subscription( + "~/input/goal", 5, std::bind(&RoutingAdaptor::on_goal, this, std::placeholders::_1)); + sub_waypoint_ = create_subscription( + "~/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(); +} + +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()); + 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()); + 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(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp new file mode 100644 index 0000000000000..dfcf5ab14922b --- /dev/null +++ b/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 +#include +#include + +#include + +#include + +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::SharedPtr cli_route_; + component_interface_utils::Client::SharedPtr cli_clear_; + rclcpp::Subscription::SharedPtr sub_goal_; + rclcpp::Subscription::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_