From 83382d632789b910a30a124b936b72419fe2dad9 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Mon, 30 Oct 2023 13:41:09 +0900 Subject: [PATCH] feat: add condition for filtering marker Signed-off-by: 1222-takeshi --- control/vehicle_cmd_gate/README.md | 1 + control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml | 3 ++- control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 6 ++++++ control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 1 + 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 406e2084d0caa..6ec2da84a7b6c 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -51,6 +51,7 @@ | `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 41161365cac6c..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,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 index ac5feb62f0737..82ba435ec6f54 100644 --- 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("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(); @@ -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 index acebb84b35e49..574591ac007a0 100644 --- 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::SharedPtr srv_engage_;