From 4a909aef2acf2a778409a746f21b4a3d52cf2823 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 27 Oct 2023 11:18:00 +0900 Subject: [PATCH 1/6] feat(vehicle_cmd_gate): improve filter marker logic Signed-off-by: 1222-takeshi --- .../config/vehicle_cmd_gate.param.yaml | 1 + .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 20 ++++++++++++++++++- .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 6 ++++++ 3 files changed, 26 insertions(+), 1 deletion(-) 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..55d61c264080f 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,7 @@ check_external_emergency_heartbeat: false use_start_request: false enable_cmd_limit_filter: true + filter_activated_count_threshold: 5 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..646eb448e05ed 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,7 @@ 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"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -572,7 +577,20 @@ 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_) { + filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated)); + is_filter_activated_flag.data = true; + } + is_filter_activated_flag.stamp = now(); + filter_activated_flag_pub_->publish(is_filter_activated_flag); return out; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 8f0a6aa14d08b..acebb84b35e49 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,7 @@ class VehicleCmdGate : public rclcpp::Node double emergency_acceleration_; double moderate_stop_service_acceleration_; bool enable_cmd_limit_filter_; + int filter_activated_count_threshold_; // Service rclcpp::Service::SharedPtr srv_engage_; From 07d8d854e18039e95d3ad4ffbff1ab84d5bd471d Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 27 Oct 2023 11:38:40 +0900 Subject: [PATCH 2/6] feat: update filtered logic Signed-off-by: 1222-takeshi --- control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 646eb448e05ed..d93af1a8be63b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -585,7 +585,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont } else { filter_activated_count_ = 0; } - if (filter_activated_count_ >= filter_activated_count_threshold_) { + if (filter_activated_count_ >= filter_activated_count_threshold_ && mode != OperationMode::AUTO) { filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated)); is_filter_activated_flag.data = true; } From 1c164710fa6b45df3dde2db40d2bd46b736026a0 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 27 Oct 2023 15:03:35 +0900 Subject: [PATCH 3/6] chore: update README.md Signed-off-by: 1222-takeshi --- control/vehicle_cmd_gate/README.md | 1 + control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 231c2c5022138..406e2084d0caa 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -50,6 +50,7 @@ | `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 | | `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/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index d93af1a8be63b..ac5feb62f0737 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -585,7 +585,9 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont } else { filter_activated_count_ = 0; } - if (filter_activated_count_ >= filter_activated_count_threshold_ && mode != OperationMode::AUTO) { + if ( + filter_activated_count_ >= filter_activated_count_threshold_ && + mode.mode == OperationModeState::AUTONOMOUS) { filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated)); is_filter_activated_flag.data = true; } From 0d1b49bd4ccf2feb708c0281859518aaf584766e Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 27 Oct 2023 15:54:57 +0900 Subject: [PATCH 4/6] feat: update parameter Signed-off-by: 1222-takeshi --- control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 55d61c264080f..41161365cac6c 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,7 @@ check_external_emergency_heartbeat: false use_start_request: false enable_cmd_limit_filter: true - filter_activated_count_threshold: 5 + filter_activated_count_threshold: 10 external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 From 83382d632789b910a30a124b936b72419fe2dad9 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Mon, 30 Oct 2023 13:41:09 +0900 Subject: [PATCH 5/6] 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_; From 00bf3d996cdebafebb9e028ef330996fab3b92c0 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Mon, 30 Oct 2023 17:18:48 +0900 Subject: [PATCH 6/6] refactor: add publishMarker function Signed-off-by: 1222-takeshi --- .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 43 ++++++++++--------- .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 1 + 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 82ba435ec6f54..1bed4ee67f412 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -579,26 +579,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont is_filter_activated.stamp = now(); is_filter_activated_pub_->publish(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); + publishMarkers(is_filter_activated); return out; } @@ -813,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 574591ac007a0..c36aab240ae79 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -243,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_; };