feat(default_ad_api): add route change api (#3197)

* feat: add route change api Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix: reroute Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
autowarefoundation · Apr 24, 2023 · cc55996 · cc55996
1 parent 16f6434
commit cc55996
Show file tree

Hide file tree

Showing 7 changed files with 35 additions and 0 deletions.
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
@@ -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
@@ -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
@@ -38,11 +38,15 @@ class RoutingNode : public rclcpp::Node
   Pub<autoware_ad_api::routing::Route> pub_route_;
   Srv<autoware_ad_api::routing::SetRoutePoints> srv_set_route_points_;
   Srv<autoware_ad_api::routing::SetRoute> srv_set_route_;
+  Srv<autoware_ad_api::routing::ChangeRoutePoints> srv_change_route_points_;
+  Srv<autoware_ad_api::routing::ChangeRoute> srv_change_route_;
   Srv<autoware_ad_api::routing::ClearRoute> srv_clear_route_;
   Sub<planning_interface::RouteState> sub_state_;
   Sub<planning_interface::Route> sub_route_;
   Cli<planning_interface::SetRoutePoints> cli_set_route_points_;
   Cli<planning_interface::SetRoute> cli_set_route_;
+  Cli<planning_interface::ChangeRoutePoints> cli_change_route_points_;
+  Cli<planning_interface::ChangeRoute> cli_change_route_;
   Cli<planning_interface::ClearRoute> cli_clear_route_;
   Cli<system_interface::ChangeOperationMode> cli_operation_mode_;
   Sub<system_interface::OperationModeState> 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
@@ -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
@@ -11,6 +11,7 @@
     <node pkg="ad_api_adaptors" exec="routing_adaptor" name="routing_adaptor">
       <remap from="~/input/fixed_goal" to="/planning/mission_planning/goal"/>
       <remap from="~/input/rough_goal" to="/rviz/routing/rough_goal"/>
+      <remap from="~/input/reroute" to="/rviz/routing/reroute"/>
       <remap from="~/input/waypoint" to="/planning/mission_planning/checkpoint"/>
     </node>
   </group>

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
@@ -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<PoseStamped>(
     "~/input/rough_goal", 3, std::bind(&RoutingAdaptor::on_rough_goal, this, _1));
+  sub_reroute_ = create_subscription<PoseStamped>(
+    "~/input/reroute", 3, std::bind(&RoutingAdaptor::on_reroute, this, _1));
   sub_waypoint_ = create_subscription<PoseStamped>(
     "~/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<SetRoutePoints::Service::Request>();
+  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
@@ -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<ChangeRoutePoints>::SharedPtr cli_reroute_;
   component_interface_utils::Client<SetRoutePoints>::SharedPtr cli_route_;
   component_interface_utils::Client<ClearRoute>::SharedPtr cli_clear_;
   component_interface_utils::Subscription<RouteState>::SharedPtr sub_state_;
   rclcpp::Subscription<PoseStamped>::SharedPtr sub_fixed_goal_;
   rclcpp::Subscription<PoseStamped>::SharedPtr sub_rough_goal_;
   rclcpp::Subscription<PoseStamped>::SharedPtr sub_waypoint_;
+  rclcpp::Subscription<PoseStamped>::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