From 6f4abd4d92a3b04d51720594bd94ee8080eab01e Mon Sep 17 00:00:00 2001
From: Tomoya Kimura <tomoya.kimura@tier4.jp>
Date: Tue, 27 Jun 2023 10:36:55 +0900
Subject: [PATCH] feat: use vehicle_stop_checker for  judging vehicle stop
 vehicle_cmd_gate (#4060)

* feat: use vehicle_stop_checker for  judging vehicle stop vehicle_cmd_gate

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* remove unused include

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

---------

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
---
 control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml | 1 +
 control/vehicle_cmd_gate/package.xml                        | 1 +
 control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp           | 6 +++++-
 control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp           | 6 ++++++
 4 files changed, 13 insertions(+), 1 deletion(-)

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 @@
   <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>
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<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.
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 <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