diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
index 9fe692e16d962..7a49d3e5f82d3 100644
--- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
+++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
@@ -10,6 +10,7 @@
emergency_acceleration: -2.4
moderate_stop_service_acceleration: -1.5
stopped_state_entry_duration_time: 0.1
+ stop_check_duration: 1.0
nominal:
vel_lim: 25.0
lon_acc_lim: 5.0
diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml
index f2a3fc83e8d63..df1de9f370cb6 100644
--- a/control/vehicle_cmd_gate/package.xml
+++ b/control/vehicle_cmd_gate/package.xml
@@ -21,6 +21,7 @@
component_interface_utils
diagnostic_updater
geometry_msgs
+ motion_utils
rclcpp
rclcpp_components
std_srvs
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
index 8af8c2426b430..25221c652e81d 100644
--- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
+++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -51,6 +51,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
rclcpp::QoS durable_qos{1};
durable_qos.transient_local();
+ // Stop Checker
+ vehicle_stop_checker_ = std::make_unique(this);
+
// Publisher
vehicle_cmd_emergency_pub_ =
create_publisher("output/vehicle_cmd_emergency", durable_qos);
@@ -145,6 +148,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
emergency_acceleration_ = declare_parameter("emergency_acceleration");
moderate_stop_service_acceleration_ =
declare_parameter("moderate_stop_service_acceleration");
+ stop_check_duration_ = declare_parameter("stop_check_duration");
// Vehicle Parameter
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -492,7 +496,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
const double dt = getDt();
const auto mode = current_operation_mode_;
const auto current_status_cmd = getActualStatusAsCommand();
- const auto ego_is_stopped = std::abs(current_status_cmd.longitudinal.speed) < 1e-3;
+ const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_);
const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0;
// Apply transition_filter when transiting from MANUAL to AUTO.
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
index 1a75d3700f346..17bfe99d45251 100644
--- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
+++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -20,6 +20,7 @@
#include "vehicle_cmd_filter.hpp"
#include
+#include
#include
#include
@@ -70,6 +71,7 @@ using nav_msgs::msg::Odometry;
using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage;
using EngageSrv = tier4_external_api_msgs::srv::Engage;
+using motion_utils::VehicleStopChecker;
struct Commands
{
AckermannControlCommand control;
@@ -219,6 +221,10 @@ class VehicleCmdGate : public rclcpp::Node
// Pause interface for API
std::unique_ptr adapi_pause_;
std::unique_ptr moderate_stop_interface_;
+
+ // stop checker
+ std::unique_ptr vehicle_stop_checker_;
+ double stop_check_duration_;
};
} // namespace vehicle_cmd_gate