Add allow_shoot mode when switching armor target.

rm-controls · Apr 15, 2024 · 691a359 · 691a359
1 parent 2af8cf0
commit 691a359
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 5 deletions.
diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h
@@ -44,6 +44,7 @@
 #include <rm_msgs/ChassisCmd.h>
 #include <rm_msgs/GimbalCmd.h>
 #include <rm_msgs/ShootCmd.h>
+#include <rm_msgs/ShootBeforehandCmd.h>
 #include <rm_msgs/GimbalDesError.h>
 #include <rm_msgs/StateCmd.h>
 #include <rm_msgs/TrackData.h>
@@ -359,7 +360,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   {
     gimbal_des_error_ = error;
   }
-  void updateAllowShoot(const rm_msgs::GimbalDesError& data)
+  void updateAllowShoot(const rm_msgs::ShootBeforehandCmd& data)
   {
     allow_shoot_ = data;
   }
@@ -373,10 +374,21 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   }
   void checkError(const ros::Time& time)
   {
+    if (msg_.mode == rm_msgs::ShootCmd::PUSH && time - allow_shoot_.stamp < ros::Duration(0.1))
+    {
+      if (allow_shoot_.cmd == rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT)
+        return;
+      if (allow_shoot_.cmd == rm_msgs::ShootBeforehandCmd::BAN_SHOOT)
+      {
+        setMode(rm_msgs::ShootCmd::READY);
+        return;
+      }
+    }
     if ((((gimbal_des_error_.error > gimbal_error_tolerance_ && time - gimbal_des_error_.stamp < ros::Duration(0.1)) ||
           (track_data_.accel > target_acceleration_tolerance_)) ||
          (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)) ||
-        (allow_shoot_.error == 0. && time - allow_shoot_.stamp < ros::Duration(0.1)))
+        (allow_shoot_.cmd == rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR &&
+         time - allow_shoot_.stamp < ros::Duration(0.1)))
       if (msg_.mode == rm_msgs::ShootCmd::PUSH)
         setMode(rm_msgs::ShootCmd::READY);
   }
@@ -464,7 +476,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   double extra_wheel_speed_once_{};
   double total_extra_wheel_speed_{};
   rm_msgs::TrackData track_data_;
-  rm_msgs::GimbalDesError gimbal_des_error_, allow_shoot_;
+  rm_msgs::GimbalDesError gimbal_des_error_;
+  rm_msgs::ShootBeforehandCmd allow_shoot_;
   std_msgs::Bool suggest_fire_;
   uint8_t armor_type_{};
 };

diff --git a/rm_msgs/msg/ShootBeforehandCmd.msg b/rm_msgs/msg/ShootBeforehandCmd.msg
@@ -1,6 +1,6 @@
 uint8 BAN_SHOOT = 0
-uint8 ALLOW_SHOOT = 1
-uint8 JUDGE_BY_ERROR = 2
+uint8 JUDGE_BY_ERROR = 1
+uint8 ALLOW_SHOOT = 2
 
 uint8 cmd