feat: add condition for filtering marker

Signed-off-by: 1222-takeshi <[email protected]>
autowarefoundation · Oct 30, 2023 · c4fab1f · c4fab1f
1 parent 0d1b49b
commit c4fab1f
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 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
@@ -6,7 +6,8 @@
     check_external_emergency_heartbeat: false
     use_start_request: false
     enable_cmd_limit_filter: true
-    filter_activated_count_threshold: 10
+    filter_activated_count_threshold: 5
+    filter_activated_velocity_threshold: 1.0
     external_emergency_stop_heartbeat_timeout: 0.0
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -165,6 +165,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   stop_check_duration_ = declare_parameter<double>("stop_check_duration");
   enable_cmd_limit_filter_ = declare_parameter<bool>("enable_cmd_limit_filter");
   filter_activated_count_threshold_ = declare_parameter<int>("filter_activated_count_threshold");
+  filter_activated_velocity_threshold_ =
+    declare_parameter<double>("filter_activated_velocity_threshold");
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -587,10 +589,14 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
   }
   if (
     filter_activated_count_ >= filter_activated_count_threshold_ &&
+    std::fabs(current_status_cmd.longitudinal.speed) >= filter_activated_velocity_threshold_ &&
     mode.mode == OperationModeState::AUTONOMOUS) {
     filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated));
     is_filter_activated_flag.data = true;
+  } else {
+    is_filter_activated_flag.data = false;
   }
+
   is_filter_activated_flag.stamp = now();
   filter_activated_flag_pub_->publish(is_filter_activated_flag);
 

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -178,6 +178,7 @@ class VehicleCmdGate : public rclcpp::Node
   double moderate_stop_service_acceleration_;
   bool enable_cmd_limit_filter_;
   int filter_activated_count_threshold_;
+  double filter_activated_velocity_threshold_;
 
   // Service
   rclcpp::Service<EngageSrv>::SharedPtr srv_engage_;