Add angle limit for shooting.

rm-controls · Apr 1, 2024 · 3c0f115 · 3c0f115
1 parent 855ed0f
commit 3c0f115
Showing 1 changed file with 9 additions and 4 deletions.
diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h
@@ -359,6 +359,10 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   {
     gimbal_des_error_ = error;
   }
+  void updateAllowShoot(const rm_msgs::GimbalDesError& data)
+  {
+    allow_shoot_ = data;
+  }
   void updateTrackData(const rm_msgs::TrackData& data)
   {
     track_data_ = data;
@@ -369,9 +373,10 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   }
   void checkError(const ros::Time& time)
   {
-    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))
+    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)))
       if (msg_.mode == rm_msgs::ShootCmd::PUSH)
         setMode(rm_msgs::ShootCmd::READY);
   }
@@ -459,7 +464,7 @@ 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_;
+  rm_msgs::GimbalDesError gimbal_des_error_, allow_shoot_;
   std_msgs::Bool suggest_fire_;
   uint8_t armor_type_{};
 };