diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index a761fa5c..6221802d 100644 --- 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 #include #include +#include #include #include #include @@ -359,9 +360,9 @@ class ShooterCommandSender : public TimeStampCommandSenderBase 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 && time - shoot_beforehand_cmd_.stamp < ros::Duration(0.1)) + { + if (shoot_beforehand_cmd_.cmd == rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT) + return; + if (shoot_beforehand_cmd_.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)) if (msg_.mode == rm_msgs::ShootCmd::PUSH) setMode(rm_msgs::ShootCmd::READY); } @@ -464,7 +474,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase