Merge pull request #237 from BlanchardLj/calculate_heat_trigger_

Calculate local heat and auto frictions' speed

rm_common/include/rm_common/decision/command_sender.h

@@ -50,6 +50,7 @@
 #include <rm_msgs/TrackData.h>
 #include <rm_msgs/GameRobotHp.h>
 #include <rm_msgs/StatusChangeRequest.h>
+#include <rm_msgs/ShootData.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <sensor_msgs/JointState.h>
 #include <nav_msgs/Odometry.h>
@@ -355,7 +356,10 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
     nh.getParam("wheel_speed_16", wheel_speed_16_);
     nh.getParam("wheel_speed_18", wheel_speed_18_);
     nh.getParam("wheel_speed_30", wheel_speed_30_);
+    nh.param("speed_oscillation", speed_oscillation_, 1.0);
     nh.param("extra_wheel_speed_once", extra_wheel_speed_once_, 0.);
+    if (!nh.getParam("auto_wheel_speed", auto_wheel_speed_))
+      ROS_INFO("auto_wheel_speed no defined (namespace: %s), set to false.", nh.getNamespace().c_str());
     if (!nh.getParam("target_acceleration_tolerance", target_acceleration_tolerance_))
     {
       target_acceleration_tolerance_ = 0.;
@@ -398,6 +402,28 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   {
     suggest_fire_ = data;
   }
+  void updateShootData(const rm_msgs::ShootData& data)
+  {
+    shoot_data_ = data;
+    if (auto_wheel_speed_)
+    {
+      if (last_bullet_speed_ == 0.0)
+        last_bullet_speed_ = speed_des_;
+      if (shoot_data_.bullet_speed != last_bullet_speed_)
+      {
+        if (last_bullet_speed_ - speed_des_ >= speed_oscillation_ || shoot_data_.bullet_speed > speed_limit_)
+        {
+          total_extra_wheel_speed_ -= 5.0;
+        }
+        else if (speed_des_ - last_bullet_speed_ > speed_oscillation_)
+        {
+          total_extra_wheel_speed_ += 5.0;
+        }
+      }
+      if (shoot_data_.bullet_speed != 0.0)
+        last_bullet_speed_ = shoot_data_.bullet_speed;
+    }
+  }
   void checkError(const ros::Time& time)
   {
     if (msg_.mode == rm_msgs::ShootCmd::PUSH && time - shoot_beforehand_cmd_.stamp < ros::Duration(0.1))
@@ -439,30 +465,35 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
     {
       case rm_msgs::ShootCmd::SPEED_10M_PER_SECOND:
       {
+        speed_limit_ = 10.0;
         speed_des_ = speed_10_;
         wheel_speed_des_ = wheel_speed_10_;
         break;
       }
       case rm_msgs::ShootCmd::SPEED_15M_PER_SECOND:
       {
+        speed_limit_ = 15.0;
         speed_des_ = speed_15_;
         wheel_speed_des_ = wheel_speed_15_;
         break;
       }
       case rm_msgs::ShootCmd::SPEED_16M_PER_SECOND:
       {
+        speed_limit_ = 16.0;
         speed_des_ = speed_16_;
         wheel_speed_des_ = wheel_speed_16_;
         break;
       }
       case rm_msgs::ShootCmd::SPEED_18M_PER_SECOND:
       {
+        speed_limit_ = 18.0;
         speed_des_ = speed_18_;
         wheel_speed_des_ = wheel_speed_18_;
         break;
       }
       case rm_msgs::ShootCmd::SPEED_30M_PER_SECOND:
       {
+        speed_limit_ = 30.0;
         speed_des_ = speed_30_;
         wheel_speed_des_ = wheel_speed_30_;
         break;
@@ -493,17 +524,19 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   HeatLimit* heat_limit_{};
 
 private:
-  double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{};
+  double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{}, speed_limit_{};
   double wheel_speed_10_{}, wheel_speed_15_{}, wheel_speed_16_{}, wheel_speed_18_{}, wheel_speed_30_{},
-      wheel_speed_des_{};
+      wheel_speed_des_{}, last_bullet_speed_{}, speed_oscillation_{};
   double track_armor_error_tolerance_{};
   double track_buff_error_tolerance_{};
   double target_acceleration_tolerance_{};
   double extra_wheel_speed_once_{};
   double total_extra_wheel_speed_{};
+  bool auto_wheel_speed_ = false;
   rm_msgs::TrackData track_data_;
   rm_msgs::GimbalDesError gimbal_des_error_;
   rm_msgs::ShootBeforehandCmd shoot_beforehand_cmd_;
+  rm_msgs::ShootData shoot_data_;
   std_msgs::Bool suggest_fire_;
   uint8_t armor_type_{};
 };

rm_common/include/rm_common/decision/heat_limit.h

@@ -41,6 +41,8 @@
 #include <rm_msgs/GameRobotStatus.h>
 #include <rm_msgs/PowerHeatData.h>
 #include <rm_msgs/ShootCmd.h>
+#include <rm_msgs/LocalHeatState.h>
+#include <std_msgs/Float64.h>
 
 namespace rm_common
 {
@@ -64,11 +66,13 @@ class HeatLimit
       ROS_ERROR("Heat coeff no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("type", type_))
       ROS_ERROR("Shooter type no defined (namespace: %s)", nh.getNamespace().c_str());
-    //    nh.param("safe_speed_limit", shooter_speed_limit_, 15);
+    nh.param("is_local", is_local_, true);
     if (type_ == "ID1_42MM")
       bullet_heat_ = 100.;
     else
       bullet_heat_ = 10.;
+    heat_pub_ = nh.advertise<std_msgs::Float64>("/local_heat_state/local_cooling_heat", 10);
+    heat_sub_ = nh.subscribe<rm_msgs::LocalHeatState>("/local_heat_state/shooter_state", 50, &HeatLimit::heatCB, this);
   }
 
   typedef enum
@@ -79,6 +83,38 @@ class HeatLimit
     MINIMAL = 3
   } ShootHz;
 
+  void heatCB(const rm_msgs::LocalHeatState msg)
+  {
+    if (msg.has_shoot == true)
+    {
+      shooter_local_cooling_heat_ += bullet_heat_;
+    }
+    if (shooter_cooling_limit_ - shooter_local_cooling_heat_ <= bullet_heat_)
+    {
+      local_frequency_ = 0.0;
+    }
+    else if (shooter_cooling_limit_ - shooter_local_cooling_heat_ <= bullet_heat_ * heat_coeff_)
+    {
+      local_frequency_ = (shooter_cooling_limit_ - shooter_local_cooling_heat_) / shooter_cooling_limit_ *
+                             (shooter_cooling_limit_ - shooter_local_cooling_heat_) / shooter_cooling_limit_ *
+                             (shoot_frequency_ - shooter_cooling_rate_ / bullet_heat_) +
+                         shooter_cooling_rate_ / bullet_heat_ + 1.0;
+    }
+    else
+    {
+      local_frequency_ = shoot_frequency_;
+    }
+    if ((ros::Time::now() - last_time_).toSec() > 0.1 && shooter_local_cooling_heat_ > 0.0)
+    {
+      shooter_local_cooling_heat_ -= shooter_cooling_rate_ / 10.0;
+      if (shooter_local_cooling_heat_ < 0.0)
+        shooter_local_cooling_heat_ = 0.0;
+      last_time_ = ros::Time::now();
+    }
+    local_heat_.data = shooter_local_cooling_heat_;
+    heat_pub_.publish(local_heat_);
+  }
+
   void setStatusOfShooter(const rm_msgs::GameRobotStatus data)
   {
     shooter_cooling_limit_ = data.shooter_cooling_limit;
@@ -110,19 +146,26 @@ class HeatLimit
   {
     if (state_ == BURST)
       return shoot_frequency_;
-    if (!referee_is_online_)
-      return safe_shoot_frequency_;
-
-    if (shooter_cooling_limit_ - shooter_cooling_heat_ < bullet_heat_)
-      return 0.0;
-    else if (shooter_cooling_limit_ - shooter_cooling_heat_ == bullet_heat_)
-      return shooter_cooling_rate_ / bullet_heat_;
-    else if (shooter_cooling_limit_ - shooter_cooling_heat_ <= bullet_heat_ * heat_coeff_)
-      return (shooter_cooling_limit_ - shooter_cooling_heat_) / (bullet_heat_ * heat_coeff_) *
-                 (shoot_frequency_ - shooter_cooling_rate_ / bullet_heat_) +
-             shooter_cooling_rate_ / bullet_heat_;
+
+    if (!is_local_)
+    {
+      if (!referee_is_online_)
+        return safe_shoot_frequency_;
+      if (shooter_cooling_limit_ - shooter_cooling_heat_ < bullet_heat_)
+        return 0.0;
+      else if (shooter_cooling_limit_ - shooter_cooling_heat_ == bullet_heat_)
+        return shooter_cooling_rate_ / bullet_heat_;
+      else if (shooter_cooling_limit_ - shooter_cooling_heat_ <= bullet_heat_ * heat_coeff_)
+        return (shooter_cooling_limit_ - shooter_cooling_heat_) / (bullet_heat_ * heat_coeff_) *
+                   (shoot_frequency_ - shooter_cooling_rate_ / bullet_heat_) +
+               shooter_cooling_rate_ / bullet_heat_;
+      else
+        return shoot_frequency_;
+    }
     else
-      return shoot_frequency_;
+    {
+      return local_frequency_;
+    }
   }
 
   int getSpeedLimit()
@@ -189,12 +232,18 @@ class HeatLimit
 
   uint8_t state_{};
   std::string type_{};
+  std_msgs::Float64 local_heat_;
   bool burst_flag_ = false;
   double bullet_heat_, safe_shoot_frequency_{}, heat_coeff_{}, shoot_frequency_{}, low_shoot_frequency_{},
-      high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{};
+      high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}, local_frequency_{};
 
-  bool referee_is_online_;
+  bool referee_is_online_, is_local_;
   int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_;
+  double shooter_local_cooling_heat_{};
+
+  ros::Publisher heat_pub_;
+  ros::Subscriber heat_sub_;
+  ros::Time last_time_{};
 };
 
 }  // namespace rm_common

rm_msgs/CMakeLists.txt

@@ -23,6 +23,7 @@ add_message_files(
         GimbalDesError.msg
         GimbalPosState.msg
         LpData.msg
+        LocalHeatState.msg
         KalmanData.msg
         MovingAverageData.msg
         GpioData.msg

rm_msgs/msg/LocalHeatState.msg

@@ -0,0 +1,3 @@
+float64 friction_change_vel
+bool has_shoot
+time stamp