feat(vehicle_cmd_gate): improve debug marker activation (#5426)

* feat(vehicle_cmd_gate): improve filter marker logic Signed-off-by: 1222-takeshi <[email protected]> * feat: update filtered logic Signed-off-by: 1222-takeshi <[email protected]> * chore: update README.md Signed-off-by: 1222-takeshi <[email protected]> * feat: update parameter Signed-off-by: 1222-takeshi <[email protected]> * feat: add condition for filtering marker Signed-off-by: 1222-takeshi <[email protected]> * refactor: add publishMarker function Signed-off-by: 1222-takeshi <[email protected]> --------- Signed-off-by: 1222-takeshi <[email protected]>
autowarefoundation · Nov 1, 2023 · 17b02d3 · 17b02d3
1 parent a2b114d
commit 17b02d3
Show file tree

Hide file tree

Showing 4 changed files with 42 additions and 1 deletion.
diff --git a/control/vehicle_cmd_gate/README.md b/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                                                                                                                  |

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,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

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/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,7 @@ 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));
+  publishMarkers(is_filter_activated);
 
   return out;
 }
@@ -787,6 +794,28 @@ MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_a
   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>

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/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_;
 };