feat(vehicle_cmd_gate): improve debug marker activation

control/vehicle_cmd_gate/README.md

@@ -50,6 +50,8 @@
 | `check_external_emergency_heartbeat`        | bool     | true when checking heartbeat for emergency stop                                                                                                                                             |
 | `system_emergency_heartbeat_timeout`        | double   | timeout for system emergency                                                                                                                                                                |
 | `external_emergency_stop_heartbeat_timeout` | double   | timeout for external emergency                                                                                                                                                              |
+| `filter_activated_count_threshold`          | int      | threshold for filter activation                                                                                                                                                             |
+| `filter_activated_velocity_threshold`       | double   | velocity threshold for filter activation                                                                                                                                                    |
 | `stop_hold_acceleration`                    | double   | longitudinal acceleration cmd when vehicle should stop                                                                                                                                      |
 | `emergency_acceleration`                    | double   | longitudinal acceleration cmd when vehicle stop with emergency                                                                                                                              |
 | `moderate_stop_service_acceleration`        | double   | longitudinal acceleration cmd when vehicle stop with moderate stop service                                                                                                                  |

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -6,6 +6,8 @@
     check_external_emergency_heartbeat: false
     use_start_request: false
     enable_cmd_limit_filter: true
+    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

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -77,6 +77,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     create_publisher<IsFilterActivated>("~/is_filter_activated", durable_qos);
   filter_activated_marker_pub_ =
     create_publisher<MarkerArray>("~/is_filter_activated/marker", durable_qos);
+  filter_activated_marker_raw_pub_ =
+    create_publisher<MarkerArray>("~/is_filter_activated/marker_raw", durable_qos);
+  filter_activated_flag_pub_ =
+    create_publisher<BoolStamped>("~/is_filter_activated/flag", durable_qos);
 
   // Subscriber
   external_emergency_stop_heartbeat_sub_ = create_subscription<Heartbeat>(
@@ -160,6 +164,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     declare_parameter<double>("moderate_stop_service_acceleration");
   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();
@@ -572,7 +579,26 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont
 
   is_filter_activated.stamp = now();
   is_filter_activated_pub_->publish(is_filter_activated);
-  filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated));
+  filter_activated_marker_raw_pub_->publish(createMarkerArray(is_filter_activated));
+
+  BoolStamped is_filter_activated_flag;
+  if (is_filter_activated.is_activated) {
+    filter_activated_count_++;
+  } else {
+    filter_activated_count_ = 0;
+  }
+  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);
 
   return out;
 }

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -62,6 +62,7 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using std_srvs::srv::Trigger;
 using tier4_control_msgs::msg::GateMode;
+using tier4_debug_msgs::msg::BoolStamped;
 using tier4_external_api_msgs::msg::Emergency;
 using tier4_external_api_msgs::msg::Heartbeat;
 using tier4_external_api_msgs::srv::SetEmergency;
@@ -106,6 +107,8 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Publisher<OperationModeState>::SharedPtr operation_mode_pub_;
   rclcpp::Publisher<IsFilterActivated>::SharedPtr is_filter_activated_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_raw_pub_;
+  rclcpp::Publisher<BoolStamped>::SharedPtr filter_activated_flag_pub_;
 
   // Subscription
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;
@@ -123,11 +126,13 @@ class VehicleCmdGate : public rclcpp::Node
   bool is_engaged_;
   bool is_system_emergency_ = false;
   bool is_external_emergency_stop_ = false;
+  bool is_filtered_marker_published_ = false;
   double current_steer_ = 0;
   GateMode current_gate_mode_;
   MrmState current_mrm_state_;
   Odometry current_kinematics_;
   double current_acceleration_ = 0.0;
+  int filter_activated_count_ = 0;
 
   // Heartbeat
   std::shared_ptr<rclcpp::Time> emergency_state_heartbeat_received_time_;
@@ -172,6 +177,8 @@ class VehicleCmdGate : public rclcpp::Node
   double emergency_acceleration_;
   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_;