fix(vehicle_cmd_gate): fix slow start (autowarefoundation#4323)

* fix(vehicle_cmd_gate): fix slow start Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * feat: consider reverse driving Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> --------- Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
tier4 · Jul 19, 2023 · 90dea56 · 90dea56
1 parent 529603b
commit 90dea56
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 2 deletions.
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp
@@ -59,6 +59,8 @@ class VehicleCmdFilter
   void filterAll(
     const double dt, const double current_steer_angle, AckermannControlCommand & input) const;
 
+  AckermannControlCommand getPrevCmd() { return prev_cmd_; }
+
 private:
   VehicleCmdFilterParam param_;
   AckermannControlCommand prev_cmd_;

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -17,6 +17,7 @@
 #include <rclcpp/logging.hpp>
 #include <tier4_api_utils/tier4_api_utils.hpp>
 
+#include <algorithm>
 #include <chrono>
 #include <functional>
 #include <memory>
@@ -494,10 +495,22 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
     filter_on_transition_.filterAll(dt, current_steer_, out);
   } else {
     // When ego is stopped and the input command is not stopping,
-    // use the actual vehicle longitudinal state for the filtering
+    // use the higher of actual vehicle longitudinal state
+    // and previous longitudinal command for the filtering
     // this is to prevent the jerk limits being applied and causing
     // a delay when restarting the vehicle.
-    if (ego_is_stopped && !input_cmd_is_stopping) filter_.setPrevCmd(current_status_cmd);
+
+    if (ego_is_stopped && !input_cmd_is_stopping) {
+      auto prev_cmd = filter_.getPrevCmd();
+      prev_cmd.longitudinal.acceleration =
+        std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration);
+      // consider reverse driving
+      prev_cmd.longitudinal.speed =
+        std::fabs(prev_cmd.longitudinal.speed) > std::fabs(current_status_cmd.longitudinal.speed)
+          ? prev_cmd.longitudinal.speed
+          : current_status_cmd.longitudinal.speed;
+      filter_.setPrevCmd(prev_cmd);
+    }
     filter_.filterAll(dt, current_steer_, out);
   }