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,89 +77,96 @@
     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>(
     "input/external_emergency_stop_heartbeat", 1,
     std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1));
   gate_mode_sub_ = create_subscription<GateMode>(
     "input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1));
   engage_sub_ = create_subscription<EngageMsg>(
     "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1));
   kinematics_sub_ = create_subscription<Odometry>(
     "/localization/kinematic_state", 1,
     [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; });
   acc_sub_ = create_subscription<AccelWithCovarianceStamped>(
     "input/acceleration", 1, [this](AccelWithCovarianceStamped::SharedPtr msg) {
       current_acceleration_ = msg->accel.accel.linear.x;
     });
   steer_sub_ = create_subscription<SteeringReport>(
     "input/steering", 1,
     [this](SteeringReport::SharedPtr msg) { current_steer_ = msg->steering_tire_angle; });
   operation_mode_sub_ = create_subscription<OperationModeState>(
     "input/operation_mode", rclcpp::QoS(1).transient_local(),
     [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; });
   mrm_state_sub_ = create_subscription<MrmState>(
     "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1));
 
   // Subscriber for auto
   auto_control_cmd_sub_ = create_subscription<AckermannControlCommand>(
     "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1));
 
   auto_turn_indicator_cmd_sub_ = create_subscription<TurnIndicatorsCommand>(
     "input/auto/turn_indicators_cmd", 1,
     [this](TurnIndicatorsCommand::ConstSharedPtr msg) { auto_commands_.turn_indicator = *msg; });
 
   auto_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
     "input/auto/hazard_lights_cmd", 1,
     [this](HazardLightsCommand::ConstSharedPtr msg) { auto_commands_.hazard_light = *msg; });
 
   auto_gear_cmd_sub_ = create_subscription<GearCommand>(
     "input/auto/gear_cmd", 1,
     [this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; });
 
   // Subscriber for external
   remote_control_cmd_sub_ = create_subscription<AckermannControlCommand>(
     "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1));
 
   remote_turn_indicator_cmd_sub_ = create_subscription<TurnIndicatorsCommand>(
     "input/external/turn_indicators_cmd", 1,
     [this](TurnIndicatorsCommand::ConstSharedPtr msg) { remote_commands_.turn_indicator = *msg; });
 
   remote_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
     "input/external/hazard_lights_cmd", 1,
     [this](HazardLightsCommand::ConstSharedPtr msg) { remote_commands_.hazard_light = *msg; });
 
   remote_gear_cmd_sub_ = create_subscription<GearCommand>(
     "input/external/gear_cmd", 1,
     [this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; });
 
   // Subscriber for emergency
   emergency_control_cmd_sub_ = create_subscription<AckermannControlCommand>(
     "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1));
 
   emergency_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
     "input/emergency/hazard_lights_cmd", 1,
     [this](HazardLightsCommand::ConstSharedPtr msg) { emergency_commands_.hazard_light = *msg; });
 
   emergency_gear_cmd_sub_ = create_subscription<GearCommand>(
     "input/emergency/gear_cmd", 1,
     [this](GearCommand::ConstSharedPtr msg) { emergency_commands_.gear = *msg; });
 
   // Parameter
   use_emergency_handling_ = declare_parameter<bool>("use_emergency_handling");
   check_external_emergency_heartbeat_ =
     declare_parameter<bool>("check_external_emergency_heartbeat");
   system_emergency_heartbeat_timeout_ =
     declare_parameter<double>("system_emergency_heartbeat_timeout");
   external_emergency_stop_heartbeat_timeout_ =
     declare_parameter<double>("external_emergency_stop_heartbeat_timeout");
   stop_hold_acceleration_ = declare_parameter<double>("stop_hold_acceleration");
   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");
   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,7 @@
 
   is_filter_activated.stamp = now();
   is_filter_activated_pub_->publish(is_filter_activated);
-  filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated));
+  publishMarkers(is_filter_activated);
 
   return out;
 }
@@ -787,6 +794,28 @@
   return msg;
 }
 
+void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated)
+{
+  BoolStamped filter_activated_flag;
+  if (filter_activated.is_activated) {
+    filter_activated_count_++;
+  } else {
+    filter_activated_count_ = 0;
+  }
+  if (
+    filter_activated_count_ >= filter_activated_count_threshold_ &&
+    std::fabs(current_kinematics_.twist.twist.linear.x) >= filter_activated_velocity_threshold_ &&
+    current_operation_mode_.mode == OperationModeState::AUTONOMOUS) {
+    filter_activated_marker_pub_->publish(createMarkerArray(filter_activated));
+    filter_activated_flag.data = true;
+  } else {
+    filter_activated_flag.data = false;
+  }
+
+  filter_activated_flag.stamp = now();
+  filter_activated_flag_pub_->publish(filter_activated_flag);
+  filter_activated_marker_raw_pub_->publish(createMarkerArray(filter_activated));
+}
 }  // namespace vehicle_cmd_gate
 
 #include <rclcpp_components/register_node_macro.hpp>

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_;
@@ -236,6 +243,7 @@ class VehicleCmdGate : public rclcpp::Node
 
   // debug
   MarkerArray createMarkerArray(const IsFilterActivated & filter_activated);
+  void publishMarkers(const IsFilterActivated & filter_activated);
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 };