feat(autoware_default_adapi): allow route clear while vehicle is stop…

…ped (autowarefoundation#10158) * feat(autoware_default_adapi): allow route clear while vehicle is stopped Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * fix parameter Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> --------- Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
tier4 · Feb 19, 2025 · c520992 · c520992
1 parent 1229329
commit c520992
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 5 deletions.
diff --git a/system/autoware_default_adapi/config/default_adapi.param.yaml b/system/autoware_default_adapi/config/default_adapi.param.yaml
@@ -7,6 +7,10 @@
     require_accept_start: false
     stop_check_duration: 1.0
 
+/adapi/node/routing:
+  ros__parameters:
+    stop_check_duration: 1.0
+
 /adapi/node/vehicle_door:
   ros__parameters:
     check_autoware_control: true
diff --git a/system/autoware_default_adapi/src/routing.cpp b/system/autoware_default_adapi/src/routing.cpp
@@ -48,8 +48,11 @@ ResponseStatus route_is_not_set()
 namespace autoware::default_adapi
 {
 
-RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", options)
+RoutingNode::RoutingNode(const rclcpp::NodeOptions & options)
+: Node("routing", options), vehicle_stop_checker_(this)
 {
+  stop_check_duration_ = declare_parameter<double>("stop_check_duration");
+
   const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this);
   group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   adaptor.init_pub(pub_state_);
@@ -124,10 +127,12 @@ void RoutingNode::on_clear_route(
   // For safety, do not clear the route while it is in use.
   // https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/clear_route/
   if (is_auto_mode_ && is_autoware_control_) {
-    res->status.success = false;
-    res->status.code = ResponseStatus::UNKNOWN;
-    res->status.message = "The route cannot be cleared while it is in use.";
-    return;
+    if (!vehicle_stop_checker_.isVehicleStopped(stop_check_duration_)) {
+      res->status.success = false;
+      res->status.code = ResponseStatus::UNKNOWN;
+      res->status.message = "The route cannot be cleared while it is in use.";
+      return;
+    }
   }
   res->status = conversion::convert_call(cli_clear_route_, req);
 }

diff --git a/system/autoware_default_adapi/src/routing.hpp b/system/autoware_default_adapi/src/routing.hpp
@@ -19,6 +19,7 @@
 #include <autoware/component_interface_specs_universe/planning.hpp>
 #include <autoware/component_interface_specs_universe/system.hpp>
 #include <autoware/component_interface_utils/status.hpp>
+#include <autoware/motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 // This file should be included after messages.
@@ -79,6 +80,10 @@ class RoutingNode : public rclcpp::Node
   void on_change_route(
     const autoware::adapi_specs::routing::SetRoute::Service::Request::SharedPtr req,
     const autoware::adapi_specs::routing::SetRoute::Service::Response::SharedPtr res);
+
+  // Stop check for route clear.
+  autoware::motion_utils::VehicleStopChecker vehicle_stop_checker_;
+  double stop_check_duration_;
 };
 
 }  // namespace autoware::default_adapi