From cc5599628c5755f1bb7cbbd13b86621935b412e4 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 24 Apr 2023 18:11:23 +0900 Subject: [PATCH] feat(default_ad_api): add route change api (#3197) * feat: add route change api Signed-off-by: Takagi, Isamu * fix: reroute Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/routing.hpp | 12 ++++++++++++ system/default_ad_api/src/routing.cpp | 2 ++ system/default_ad_api/src/routing.hpp | 4 ++++ .../default_ad_api_helpers/ad_api_adaptors/README.md | 1 + .../ad_api_adaptors/launch/rviz_adaptors.launch.xml | 1 + .../ad_api_adaptors/src/routing_adaptor.cpp | 11 +++++++++++ .../ad_api_adaptors/src/routing_adaptor.hpp | 4 ++++ 7 files changed, 35 insertions(+) diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp index 76fac74f667f9..62f99a4a8d8e9 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp @@ -38,6 +38,18 @@ struct SetRoute static constexpr char name[] = "/api/routing/set_route"; }; +struct ChangeRoutePoints +{ + using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; + static constexpr char name[] = "/api/routing/change_route_points"; +}; + +struct ChangeRoute +{ + using Service = autoware_adapi_v1_msgs::srv::SetRoute; + static constexpr char name[] = "/api/routing/change_route"; +}; + struct ClearRoute { using Service = autoware_adapi_v1_msgs::srv::ClearRoute; diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index f2181eea5fb74..5204146201c8a 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -33,6 +33,8 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_srv(srv_clear_route_, this, &RoutingNode::on_clear_route); adaptor.relay_service(cli_set_route_, srv_set_route_, group_cli_); adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_); + adaptor.relay_service(cli_change_route_, srv_change_route_, group_cli_); + adaptor.relay_service(cli_change_route_points_, srv_change_route_points_, group_cli_); adaptor.init_cli(cli_operation_mode_, group_cli_); adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp index a0b1626d42807..7c15f6d64608f 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/default_ad_api/src/routing.hpp @@ -38,11 +38,15 @@ class RoutingNode : public rclcpp::Node Pub pub_route_; Srv srv_set_route_points_; Srv srv_set_route_; + Srv srv_change_route_points_; + Srv srv_change_route_; Srv srv_clear_route_; Sub sub_state_; Sub sub_route_; Cli cli_set_route_points_; Cli cli_set_route_; + Cli cli_change_route_points_; + Cli cli_change_route_; Cli cli_clear_route_; Cli cli_operation_mode_; Sub sub_operation_mode_; diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index b0dd92df3a95b..6cb43c5475828 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -24,6 +24,7 @@ The clear API is called automatically before setting the route. | ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- | | Subscription | ~/input/fixed_goal | /planning/mission_planning/goal | The goal pose of route. Disable goal modification. | | Subscription | ~/input/rough_goal | /rviz/routing/rough_goal | The goal pose of route. Enable goal modification. | +| Subscription | ~/input/reroute | /rviz/routing/reroute | The goal pose of reroute. | | 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 index 02f873813520c..4ed9d134d2183 100644 --- 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 @@ -11,6 +11,7 @@ + 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 index 4823955474d06..7151902a972d6 100644 --- 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 @@ -27,10 +27,13 @@ RoutingAdaptor::RoutingAdaptor() : Node("routing_adaptor") "~/input/fixed_goal", 3, std::bind(&RoutingAdaptor::on_fixed_goal, this, _1)); sub_rough_goal_ = create_subscription( "~/input/rough_goal", 3, std::bind(&RoutingAdaptor::on_rough_goal, this, _1)); + sub_reroute_ = create_subscription( + "~/input/reroute", 3, std::bind(&RoutingAdaptor::on_reroute, this, _1)); sub_waypoint_ = create_subscription( "~/input/waypoint", 10, std::bind(&RoutingAdaptor::on_waypoint, this, _1)); const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_cli(cli_reroute_); adaptor.init_cli(cli_route_); adaptor.init_cli(cli_clear_); adaptor.init_sub( @@ -97,6 +100,14 @@ void RoutingAdaptor::on_waypoint(const PoseStamped::ConstSharedPtr pose) route_->waypoints.push_back(pose->pose); } +void RoutingAdaptor::on_reroute(const PoseStamped::ConstSharedPtr pose) +{ + const auto route = std::make_shared(); + route->header = pose->header; + route->goal = pose->pose; + cli_reroute_->async_send_request(route); +} + } // namespace ad_api_adaptors int main(int argc, char ** argv) 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 index 6c0bbf72e40a7..4040ee37ead3e 100644 --- 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 @@ -34,14 +34,17 @@ class RoutingAdaptor : public rclcpp::Node private: using PoseStamped = geometry_msgs::msg::PoseStamped; using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints; + using ChangeRoutePoints = autoware_ad_api::routing::ChangeRoutePoints; using ClearRoute = autoware_ad_api::routing::ClearRoute; using RouteState = autoware_ad_api::routing::RouteState; + component_interface_utils::Client::SharedPtr cli_reroute_; component_interface_utils::Client::SharedPtr cli_route_; component_interface_utils::Client::SharedPtr cli_clear_; component_interface_utils::Subscription::SharedPtr sub_state_; rclcpp::Subscription::SharedPtr sub_fixed_goal_; rclcpp::Subscription::SharedPtr sub_rough_goal_; rclcpp::Subscription::SharedPtr sub_waypoint_; + rclcpp::Subscription::SharedPtr sub_reroute_; rclcpp::TimerBase::SharedPtr timer_; bool calling_service_ = false; @@ -53,6 +56,7 @@ class RoutingAdaptor : public rclcpp::Node void on_fixed_goal(const PoseStamped::ConstSharedPtr pose); void on_rough_goal(const PoseStamped::ConstSharedPtr pose); void on_waypoint(const PoseStamped::ConstSharedPtr pose); + void on_reroute(const PoseStamped::ConstSharedPtr pose); }; } // namespace ad_api_adaptors