Merge pull request #690 from tier4/fix/fix-judging-vehicle-stop-state

feat(vehicle_cmd_gate): use vehicle_stop_checker for  judging vehicle stop

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

control/vehicle_cmd_gate/package.xml

@@ -21,6 +21,7 @@
   <depend>component_interface_utils</depend>
   <depend>diagnostic_updater</depend>
   <depend>geometry_msgs</depend>
+  <depend>motion_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>std_srvs</depend>

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<VehicleStopChecker>(this);
+
   // Publisher
   vehicle_cmd_emergency_pub_ =
     create_publisher<VehicleEmergencyStamped>("output/vehicle_cmd_emergency", durable_qos);
@@ -145,6 +148,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   emergency_acceleration_ = declare_parameter<double>("emergency_acceleration");
   moderate_stop_service_acceleration_ =
     declare_parameter<double>("moderate_stop_service_acceleration");
+  stop_check_duration_ = declare_parameter<double>("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.

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -20,6 +20,7 @@
 #include "vehicle_cmd_filter.hpp"
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
+#include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
@@ -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<AdapiPauseInterface> adapi_pause_;
   std::unique_ptr<ModerateStopInterface> moderate_stop_interface_;
+
+  // stop checker
+  std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
+  double stop_check_duration_;
 };
 
 }  // namespace vehicle_cmd_gate