Skip to content

Commit

Permalink
Merge pull request rm-controls#140 from ye-luo-xi-tui/master
Browse files Browse the repository at this point in the history 
Add function that tracking center of the enemy when enemy's angular velocity is too large.
  • Loading branch information
@ye-luo-xi-tui
ye-luo-xi-tui authored Aug 4, 2023
2 parents 1c37b64 + e87fd00 commit 1a76190
Show file tree
Hide file tree
Showing 2 changed files with 81 additions and 43 deletions.
3 changes: 3 additions & 0 deletions rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,12 +86,15 @@ class BulletSolver
realtime_tools::RealtimeBuffer<Config> config_rt_buffer_;
dynamic_reconfigure::Server<rm_gimbal_controllers::BulletSolverConfig>* d_srv_{};
Config config_{};
double max_track_target_vel_;
bool dynamic_reconfig_initialized_{};
double output_yaw_{}, output_pitch_{};
double bullet_speed_{}, resistance_coff_{};
int selected_armor_;
bool track_target_;

geometry_msgs::Point target_pos_{};
double fly_time_;
visualization_msgs::Marker marker_desire_;
visualization_msgs::Marker marker_real_;
};
Expand Down
121 changes: 78 additions & 43 deletions rm_gimbal_controllers/src/bullet_solver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
.delay = getParam(controller_nh, "delay", 0.),
.dt = getParam(controller_nh, "dt", 0.),
.timeout = getParam(controller_nh, "timeout", 0.) };
max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0);
config_rt_buffer_.initRT(config_);

marker_desire_.header.frame_id = "odom";
Expand Down Expand Up @@ -114,33 +115,60 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
selected_armor_ = 0;
double r = r1;
double z = pos.z;
if (((yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4) || ((yaw + v_yaw * rough_fly_time) < output_yaw_ - M_PI_4))
track_target_ = std::abs(v_yaw) < max_track_target_vel_;
double switch_armor_angle =
track_target_ ?
acos(r / target_rho) - M_PI / 12 + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ :
M_PI / 12;
if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) ||
(((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.))
{
selected_armor_ = (yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4 ? -1 : 1;
selected_armor_ = v_yaw > 0. ? -1 : 1;
r = armors_num == 4 ? r2 : r1;
z = armors_num == 4 ? pos.z + dz : pos.z;
}
int count{};
double error = 999;
target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
if (track_target_)
{
target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
}
else
{
target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x));
target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x));
}
target_pos_.z = z;
while (error >= 0.001)
{
output_yaw_ = std::atan2(target_pos_.y, target_pos_.x);
output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)));
target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2));
double fly_time =
fly_time_ =
(-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_;
double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) *
(1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ -
config_.g * fly_time / resistance_coff_;
(1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ -
config_.g * fly_time_ / resistance_coff_;

target_pos_.x =
pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num);
target_pos_.y =
pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num);
target_pos_.z = z + vel.z * fly_time;
if (track_target_)
{
target_pos_.x =
pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num);
target_pos_.y =
pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num);
}
else
{
double target_pos_after_fly_time[2];
target_pos_after_fly_time[0] = pos.x + vel.x * fly_time_;
target_pos_after_fly_time[1] = pos.y + vel.y * fly_time_;
target_pos_.x =
target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0]));
target_pos_.y =
target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0]));
}
target_pos_.z = z + vel.z * fly_time_;

double target_yaw = std::atan2(target_pos_.y, target_pos_.x);
double error_theta = target_yaw - output_yaw_;
Expand All @@ -165,12 +193,21 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge
r = r2;
z = pos.z + dz;
}
armor_pos.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_pos.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_pos.z = z;
armor_vel.x = vel.x + v_yaw * r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_vel.y = vel.y - v_yaw * r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_vel.z = vel.z;
if (track_target_)
{
armor_pos.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_pos.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_pos.z = z;
armor_vel.x = vel.x + v_yaw * r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_vel.y = vel.y - v_yaw * r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
armor_vel.z = vel.z;
}
else
{
armor_pos = pos;
armor_pos.z = z;
armor_vel = vel;
}
}

void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time)
Expand Down Expand Up @@ -226,40 +263,38 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec
double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real,
double bullet_speed)
{
config_ = *config_rt_buffer_.readFromRT();
double resistance_coff = getResistanceCoefficient(bullet_speed);
double delay = track_target_ ? 0 : config_.delay;
double r = r1;
double z = pos.z;
if (selected_armor_ != 0)
{
r = armors_num == 4 ? r2 : r1;
z = armors_num == 4 ? pos.z + dz : pos.z;
}
double fly_time = (-std::log(1 - std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)) * resistance_coff /
(bullet_speed * std::cos(pitch_real)))) /
resistance_coff;
double last_fly_time{}, target_rho{};
int count{};
geometry_msgs::Point target_pos{};
while (std::abs(fly_time - last_fly_time) > 0.01)
double error;
if (track_target_)
{
last_fly_time = fly_time;
target_pos.x = pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num);
target_pos.y = pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num);
target_pos.z = z + vel.z * fly_time;
target_rho = std::sqrt(std::pow(target_pos.x, 2) + std::pow(target_pos.y, 2));
fly_time = (-std::log(1 - target_rho * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / resistance_coff;
count++;
if (count >= 20 || std::isnan(fly_time))
return 999;
double bullet_rho =
bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_;
double bullet_x = bullet_rho * std::cos(yaw_real);
double bullet_y = bullet_rho * std::sin(yaw_real);
double bullet_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff_)) *
(1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ -
config_.g * fly_time_ / resistance_coff_;
error = std::sqrt(std::pow(target_pos_.x - bullet_x, 2) + std::pow(target_pos_.y - bullet_y, 2) +
std::pow(target_pos_.z - bullet_z, 2));
}
else
{
geometry_msgs::Point target_pos_after_fly_time_and_delay{};
target_pos_after_fly_time_and_delay.x = pos.x + vel.x * (fly_time_ + delay) -
r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num);
target_pos_after_fly_time_and_delay.y = pos.y + vel.y * (fly_time_ + delay) -
r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num);
target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay);
error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x,2) + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y,2) +
std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z,2));
}
double real_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff)) *
(1 - std::exp(-resistance_coff * fly_time)) / resistance_coff -
config_.g * fly_time / resistance_coff;
double target_yaw = std::atan2(target_pos.y, target_pos.x);
double error = std::sqrt(std::pow(target_rho * (std::cos(target_yaw) - std::cos(yaw_real)), 2) +
std::pow(target_rho * (std::sin(target_yaw) - std::sin(yaw_real)), 2) +
std::pow(target_pos.z - real_z, 2));
return error;
}

Expand Down

0 comments on commit 1a76190

Please sign in to comment.