feat(mission_planner): use ClearRoute instead of Trigger

Signed-off-by: kosuke55 <[email protected]>
TomohitoAndo · Dec 4, 2023 · 10dbca2 · 10dbca2
1 parent 8b59d16
commit 10dbca2
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 9 deletions.
diff --git a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp
@@ -32,7 +32,7 @@ struct SetMrmRoute
 
 struct ClearMrmRoute
 {
-  using Service = std_srvs::srv::Trigger;
+  using Service = autoware_adapi_v1_msgs::srv::ClearRoute;
   static constexpr char name[] = "~/srv/clear_mrm_route";
 };
 

diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -497,7 +497,7 @@ void MissionPlanner::on_clear_mrm_route(
   if (!normal_route_) {
     clear_mrm_route();
     change_state(RouteState::Message::UNSET);
-    res->success = true;
+    res->status.success = true;
     return;
   }
 
@@ -506,7 +506,7 @@ void MissionPlanner::on_clear_mrm_route(
     clear_mrm_route();
     change_route(*normal_route_);
     change_state(RouteState::Message::SET);
-    res->success = true;
+    res->status.success = true;
     return;
   }
 
@@ -523,12 +523,12 @@ void MissionPlanner::on_clear_mrm_route(
     change_mrm_route(*mrm_route_);
     change_route(*normal_route_);
     change_state(RouteState::Message::SET);
-    res->success = false;
+    res->status.success = false;
   } else {
     clear_mrm_route();
     change_route(new_route);
     change_state(RouteState::Message::SET);
-    res->success = true;
+    res->status.success = true;
   }
 }
 

diff --git a/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp b/system/emergency_goal_manager/include/emergency_goal_manager_core.hpp
@@ -16,6 +16,7 @@
 #define EMERGENCY_GOAL_MANAGER_CORE_HPP_
 
 // Autoware
+#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
 #include <tier4_system_msgs/msg/emergency_goals_clear_command.hpp>
 #include <tier4_system_msgs/msg/emergency_goals_stamped.hpp>
@@ -39,6 +40,7 @@ class EmergencyGoalManager : public rclcpp::Node
 
 private:
   using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+  using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute;
 
   // Subscriber
   rclcpp::Subscription<tier4_system_msgs::msg::EmergencyGoalsStamped>::SharedPtr
@@ -54,7 +56,7 @@ class EmergencyGoalManager : public rclcpp::Node
   rclcpp::CallbackGroup::SharedPtr client_set_mrm_route_points_callback_group_;
   rclcpp::Client<SetRoutePoints>::SharedPtr client_set_mrm_route_points_;
   rclcpp::CallbackGroup::SharedPtr client_clear_mrm_route_callback_group_;
-  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client_clear_mrm_route_;
+  rclcpp::Client<ClearRoute>::SharedPtr client_clear_mrm_route_;
 
   // Variables
   std::unordered_map<std::string, std::queue<geometry_msgs::msg::Pose>> emergency_goals_map_;

diff --git a/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp b/system/emergency_goal_manager/src/emergency_goal_manager_core.cpp
@@ -35,7 +35,7 @@ EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager")
     rmw_qos_profile_services_default, client_set_mrm_route_points_callback_group_);
   client_clear_mrm_route_callback_group_ =
     create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  client_clear_mrm_route_ = create_client<std_srvs::srv::Trigger>(
+  client_clear_mrm_route_ = create_client<ClearRoute>(
     "/planning/mission_planning/mission_planner/srv/clear_mrm_route",
     rmw_qos_profile_services_default, client_clear_mrm_route_callback_group_);
 
@@ -122,7 +122,7 @@ void EmergencyGoalManager::callSetMrmRoutePoints()
 
 void EmergencyGoalManager::callClearMrmRoute()
 {
-  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
+  auto request = std::make_shared<ClearRoute::Request>();
   const auto duration = std::chrono::duration<double, std::ratio<1>>(10);
 
   while (true) {
@@ -132,7 +132,7 @@ void EmergencyGoalManager::callClearMrmRoute()
       RCLCPP_WARN(get_logger(), "Clear MRM Route service timeout.");
       return;
     } else {
-      if (future.get()->success) {
+      if (future.get()->status.success) {
         RCLCPP_INFO(get_logger(), "Clear MRM Route has been successfully sent.");
         return;
       } else {