Skip to content

Commit

Permalink
Revert "Calculate local heat. "
Browse files Browse the repository at this point in the history
  • Loading branch information
@d0h0s
d0h0s authored Jul 25, 2024
1 parent 37807c8 commit 077bc32
Show file tree
Hide file tree
Showing 4 changed files with 17 additions and 104 deletions.
37 changes: 2 additions & 35 deletions rm_common/include/rm_common/decision/command_sender.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,6 @@
#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>
Expand Down Expand Up @@ -356,10 +355,7 @@ 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.;
Expand Down Expand Up @@ -402,28 +398,6 @@ 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))
Expand Down Expand Up @@ -465,35 +439,30 @@ 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;
Expand Down Expand Up @@ -524,19 +493,17 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
HeatLimit* heat_limit_{};

private:
double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{}, speed_limit_{};
double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{};
double wheel_speed_10_{}, wheel_speed_15_{}, wheel_speed_16_{}, wheel_speed_18_{}, wheel_speed_30_{},
wheel_speed_des_{}, last_bullet_speed_{}, speed_oscillation_{};
wheel_speed_des_{};
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_{};
};
Expand Down
79 changes: 15 additions & 64 deletions rm_common/include/rm_common/decision/heat_limit.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,6 @@
#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
{
Expand All @@ -66,13 +64,11 @@ 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("is_local", is_local_, true);
// nh.param("safe_speed_limit", shooter_speed_limit_, 15);
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
Expand All @@ -83,38 +79,6 @@ 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;
Expand Down Expand Up @@ -146,26 +110,19 @@ class HeatLimit
{
if (state_ == BURST)
return shoot_frequency_;

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_;
}
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 local_frequency_;
}
return shoot_frequency_;
}

int getSpeedLimit()
Expand Down Expand Up @@ -232,18 +189,12 @@ 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_{}, local_frequency_{};
high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{};

bool referee_is_online_, is_local_;
bool referee_is_online_;
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
1 change: 0 additions & 1 deletion rm_msgs/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@ add_message_files(
GimbalDesError.msg
GimbalPosState.msg
LpData.msg
LocalHeatState.msg
KalmanData.msg
MovingAverageData.msg
GpioData.msg
Expand Down
4 changes: 0 additions & 4 deletions rm_msgs/msg/LocalHeatState.msg
View file

This file was deleted.

0 comments on commit 077bc32

Please sign in to comment.