Skip to content

Commit

Permalink
Merge branch 'master' of github.com:GuraMumei/rm_control
Browse files Browse the repository at this point in the history
  • Loading branch information
GuraMumei committed May 5, 2024
2 parents b15fdbf + 6f933b8 commit 536d853
Showing 3 changed files with 27 additions and 8 deletions.
25 changes: 18 additions & 7 deletions rm_common/include/rm_common/decision/command_sender.h
Original file line number Diff line number Diff line change
@@ -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,9 +360,9 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
{
gimbal_des_error_ = error;
}
void updateAllowShoot(const rm_msgs::GimbalDesError& data)
void updateShootBeforehandCmd(const rm_msgs::ShootBeforehandCmd& data)
{
allow_shoot_ = data;
shoot_beforehand_cmd_ = data;
}
void updateTrackData(const rm_msgs::TrackData& data)
{
@@ -373,10 +374,19 @@ 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)) ||
(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<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 shoot_beforehand_cmd_;
std_msgs::Bool suggest_fire_;
uint8_t armor_type_{};
};
3 changes: 2 additions & 1 deletion rm_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS
geometry_msgs
actionlib
actionlib_msgs
)
)

add_message_files(
FILES
@@ -17,6 +17,7 @@ add_message_files(
ChassisCmd.msg
ShootCmd.msg
ShootState.msg
ShootBeforehandCmd.msg
GimbalCmd.msg
GimbalDesError.msg
LpData.msg
7 changes: 7 additions & 0 deletions rm_msgs/msg/ShootBeforehandCmd.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
uint8 BAN_SHOOT = 0
uint8 JUDGE_BY_ERROR = 1
uint8 ALLOW_SHOOT = 2

uint8 cmd

time stamp

0 comments on commit 536d853

Please sign in to comment.