diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 231c2c5022138..6ec2da84a7b6c 100644 --- 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 index 191e894622af7..7ad685217f13d 100644 --- 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 index e681bc22cd24b..1bed4ee67f412 100644 --- 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("~/is_filter_activated", durable_qos); filter_activated_marker_pub_ = create_publisher("~/is_filter_activated/marker", durable_qos); + filter_activated_marker_raw_pub_ = + create_publisher("~/is_filter_activated/marker_raw", durable_qos); + filter_activated_flag_pub_ = + create_publisher("~/is_filter_activated/flag", durable_qos); // Subscriber external_emergency_stop_heartbeat_sub_ = create_subscription( @@ -160,6 +164,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) declare_parameter("moderate_stop_service_acceleration"); stop_check_duration_ = declare_parameter("stop_check_duration"); enable_cmd_limit_filter_ = declare_parameter("enable_cmd_limit_filter"); + filter_activated_count_threshold_ = declare_parameter("filter_activated_count_threshold"); + filter_activated_velocity_threshold_ = + declare_parameter("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 diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 8f0a6aa14d08b..c36aab240ae79 100644 --- 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::SharedPtr operation_mode_pub_; rclcpp::Publisher::SharedPtr is_filter_activated_pub_; rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; + rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; + rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; // Subscription rclcpp::Subscription::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 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::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 logger_configure_; };