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