From 5bfeca832ae275b40e8b7a4ee5dcb686de5523f7 Mon Sep 17 00:00:00 2001
From: chen-gr <725393026@qq.com>
Date: Sun, 1 Jun 2025 21:46:08 +0800
Subject: [PATCH 1/2] Optimize the judgment method of local heat.

---
 .../include/rm_shooter_controllers/standard.h |  7 ++-
 rm_shooter_controllers/src/standard.cpp       | 51 +++++++------------
 2 files changed, 23 insertions(+), 35 deletions(-)

diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
index 2a2b4f70..26cd27fb 100644
--- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
+++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
@@ -43,6 +43,7 @@
 #include <hardware_interface/joint_command_interface.h>
 #include <rm_common/hardware_interface/robot_state_interface.h>
 #include <rm_common/ros_utilities.h>
+#include <rm_common/filters/lp_filter.h>
 #include <realtime_tools/realtime_publisher.h>
 #include <dynamic_reconfigure/server.h>
 #include <rm_shooter_controllers/ShooterConfig.h>
@@ -90,6 +91,7 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
   effort_controllers::JointPositionController ctrl_trigger_;
   std::vector<std::vector<double>> wheel_speed_offsets_;
   std::vector<std::vector<double>> wheel_speed_directions_;
+  LowPassFilter *lp_filter_;
   int push_per_rotation_{}, count_{};
   double push_wheel_speed_threshold_{};
   double freq_threshold_{};
@@ -102,8 +104,9 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
   double friction_block_effort_{}, friction_block_vel_{};
   double anti_friction_block_duty_cycle_{}, anti_friction_block_vel_{};
   bool has_shoot_ = false, has_shoot_last_ = false;
-  bool wheel_speed_raise_ = true, wheel_speed_drop_ = true;
-  double last_wheel_speed_{};
+  bool wheel_speed_drop_ = true;
+  double last_fricition_speed_{};
+  double last_friction_change_speed_{};
 
   ros::Time last_shoot_time_, block_time_, last_block_time_;
   enum
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index 2afb7a5c..58d5edba 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -101,6 +101,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
     wheel_speed_offsets_.push_back(wheel_speed_offset_temp);
     wheel_speed_directions_.push_back(wheel_speed_direction_temp);
   }
+  lp_filter_ = new LowPassFilter(controller_nh);
   ros::NodeHandle nh_trigger = ros::NodeHandle(controller_nh, "trigger");
   return ctrl_trigger_.init(effort_joint_interface_, nh_trigger);
 }
@@ -338,45 +339,29 @@ void Controller::normalize()
 
 void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period)
 {
+  lp_filter_->input(ctrls_friction_[0][0]->joint_.getVelocity());
+  double friction_speed = lp_filter_->output();
+  double friction_change_speed = abs(friction_speed) - last_fricition_speed_;
+  double friction_change_speed_derivative = friction_change_speed - last_friction_change_speed_;
   if (state_ != STOP)
   {
-    if (abs(ctrls_friction_[0][0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold &&
-        wheel_speed_drop_)
-    {
-      wheel_speed_raise_ = true;
-      wheel_speed_drop_ = false;
-    }
-
-    if (last_wheel_speed_ - abs(ctrls_friction_[0][0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold &&
-        abs(ctrls_friction_[0][0]->joint_.getVelocity()) > 300. && wheel_speed_raise_)
-    {
-      wheel_speed_drop_ = true;
-      wheel_speed_raise_ = false;
+    if (friction_change_speed_derivative > 0 && has_shoot_)
+      has_shoot_ = false;
+    if (friction_change_speed < -config_.wheel_speed_drop_threshold  && !has_shoot_ && friction_change_speed_derivative < 0)
       has_shoot_ = true;
-    }
   }
-  double friction_change_vel = abs(ctrls_friction_[0][0]->joint_.getVelocity()) - last_wheel_speed_;
-  last_wheel_speed_ = abs(ctrls_friction_[0][0]->joint_.getVelocity());
-  count_++;
-  if (has_shoot_last_)
-  {
-    has_shoot_ = true;
-  }
-  has_shoot_last_ = has_shoot_;
-  if (count_ == 2)
+  last_fricition_speed_ = abs(friction_speed);
+  last_friction_change_speed_ = friction_change_speed;
+
+  if (local_heat_state_pub_->trylock())
   {
-    if (local_heat_state_pub_->trylock())
-    {
-      local_heat_state_pub_->msg_.stamp = time;
-      local_heat_state_pub_->msg_.has_shoot = has_shoot_;
-      local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel;
-      local_heat_state_pub_->unlockAndPublish();
-    }
-    has_shoot_last_ = false;
-    count_ = 0;
+    local_heat_state_pub_->msg_.stamp = time;
+    local_heat_state_pub_->msg_.has_shoot = has_shoot_;
+    local_heat_state_pub_->msg_.friction_speed = friction_speed;
+    local_heat_state_pub_->msg_.friction_change_speed = friction_change_speed;
+    local_heat_state_pub_->msg_.friction_change_speed_derivative = friction_change_speed_derivative;
+    local_heat_state_pub_->unlockAndPublish();
   }
-  if (has_shoot_)
-    has_shoot_ = false;
 }
 void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/)
 {

From ece205b9948cfb535476ea4dcc7d798cf5d82a1f Mon Sep 17 00:00:00 2001
From: chen-gr <725393026@qq.com>
Date: Sun, 1 Jun 2025 21:58:46 +0800
Subject: [PATCH 2/2] Fix code format.

---
 .../include/rm_shooter_controllers/standard.h                  | 2 +-
 rm_shooter_controllers/src/standard.cpp                        | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
index 26cd27fb..bf07962d 100644
--- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
+++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
@@ -91,7 +91,7 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
   effort_controllers::JointPositionController ctrl_trigger_;
   std::vector<std::vector<double>> wheel_speed_offsets_;
   std::vector<std::vector<double>> wheel_speed_directions_;
-  LowPassFilter *lp_filter_;
+  LowPassFilter* lp_filter_;
   int push_per_rotation_{}, count_{};
   double push_wheel_speed_threshold_{};
   double freq_threshold_{};
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index 58d5edba..c2e0d541 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -347,7 +347,8 @@ void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& pe
   {
     if (friction_change_speed_derivative > 0 && has_shoot_)
       has_shoot_ = false;
-    if (friction_change_speed < -config_.wheel_speed_drop_threshold  && !has_shoot_ && friction_change_speed_derivative < 0)
+    if (friction_change_speed < -config_.wheel_speed_drop_threshold && !has_shoot_ &&
+        friction_change_speed_derivative < 0)
       has_shoot_ = true;
   }
   last_fricition_speed_ = abs(friction_speed);