cyy/yq_2026_duo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化

Browse Source
This commit is contained in:
cyy_mac
2026-03-25 17:07:08 +08:00
parent 57a1c467d1
commit 9b9c603134
7 changed files with 124 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp
View File

@@ -50,6 +50,9 @@ public:
enum State { TRACKING_ARMOR = 0, TRACKING_CENTER = 1 } state;
std::vector<std::pair<double, double>> getTrajectory() const noexcept;
const std::vector<Eigen::Vector3d>& getArmorPositions() const noexcept {
return cached_armor_positions_;
}
void setBulletSpeed(double bullet_speed) noexcept;
void updateRuntimeParams(double max_tracking_v_yaw,
double prediction_delay,
@@ -97,6 +100,12 @@ private:
std::array<double, 3> rpy_;
// Cached computation results to avoid redundant calculations
mutable std::vector<Eigen::Vector3d> cached_armor_positions_;
mutable std::vector<std::pair<double, double>> cached_trajectory_;
mutable bool trajectory_cache_valid_ = false;
mutable double cached_trajectory_pitch_ = 0.;
double prediction_delay_;
double controller_delay_;

48
src/rm_auto_aim/armor_solver/src/armor_solver.cpp
View File

@@ -66,6 +66,9 @@ Solver::Solver(std::weak_ptr<rclcpp::Node> n) : node_(n) {
overflow_count_ = 0;
transfer_thresh_ = 5;
// Initialize cache
trajectory_cache_valid_ = false;
node.reset();
}
@@ -140,16 +143,16 @@ rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &ta
target_yaw += dt * target.v_yaw;
// Choose the best armor to shoot
std::vector<Eigen::Vector3d> armor_positions = getArmorPositions(target_position,
target_yaw,
target.radius_1,
target.radius_2,
target.d_zc,
target.d_za,
target.armors_num);
cached_armor_positions_ = getArmorPositions(target_position,
target_yaw,
target.radius_1,
target.radius_2,
target.d_zc,
target.d_za,
target.armors_num);
int idx =
selectBestArmor(armor_positions, target_position, target_yaw, target.v_yaw, target.armors_num);
auto chosen_armor_position = armor_positions.at(idx);
selectBestArmor(cached_armor_positions_, target_position, target_yaw, target.v_yaw, target.armors_num);
auto chosen_armor_position = cached_armor_positions_.at(idx);
if (chosen_armor_position.norm() < 0.1) {
throw std::runtime_error("No valid armor to shoot");
}
@@ -183,14 +186,14 @@ rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &ta
target_position.y() += controller_delay_ * target.velocity.y;
target_position.z() += controller_delay_ * target.velocity.z;
target_yaw += controller_delay_ * target.v_yaw;
armor_positions = getArmorPositions(target_position,
cached_armor_positions_ = getArmorPositions(target_position,
target_yaw,
target.radius_1,
target.radius_2,
target.d_zc,
target.d_za,
target.armors_num);
chosen_armor_position = armor_positions.at(idx);
chosen_armor_position = cached_armor_positions_.at(idx);
gimbal_cmd.distance = chosen_armor_position.norm();
if (chosen_armor_position.norm() < 0.1) {
throw std::runtime_error("No valid armor to shoot");
@@ -372,8 +375,26 @@ void Solver::calcYawAndPitch(const Eigen::Vector3d &p,
}
std::vector<std::pair<double, double>> Solver::getTrajectory() const noexcept {
auto trajectory = trajectory_compensator_->getTrajectory(15, rpy_[1]);
// Rotate
// Use cached trajectory if pitch hasn't changed
if (trajectory_cache_valid_ && std::abs(cached_trajectory_pitch_ - rpy_[1]) < 1e-6) {
// Return cached trajectory with rotation applied
auto trajectory = cached_trajectory_;
for (auto &p : trajectory) {
double x = p.first;
double y = p.second;
p.first = x * cos(rpy_[1]) + y * sin(rpy_[1]);
p.second = -x * sin(rpy_[1]) + y * cos(rpy_[1]);
}
return trajectory;
}
// Compute new trajectory and cache it
cached_trajectory_ = trajectory_compensator_->getTrajectory(15, rpy_[1]);
cached_trajectory_pitch_ = rpy_[1];
trajectory_cache_valid_ = true;
// Return with rotation applied
auto trajectory = cached_trajectory_;
for (auto &p : trajectory) {
double x = p.first;
double y = p.second;
@@ -386,6 +407,7 @@ std::vector<std::pair<double, double>> Solver::getTrajectory() const noexcept {
void Solver::setBulletSpeed(double bullet_speed) noexcept {
if (std::isfinite(bullet_speed) && bullet_speed > 0.0) {
trajectory_compensator_->velocity = bullet_speed;
trajectory_cache_valid_ = false; // Invalidate cache when bullet speed changes
}
}

50
src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp
View File

@@ -454,39 +454,39 @@ void ArmorSolverNode::publishMarkers(const rm_interfaces::msg::Target &target_ms
angular_v_marker_.points.emplace_back(arrow_end);
armors_marker_.action = visualization_msgs::msg::Marker::ADD;
armors_marker_.scale.y = tracker_->tracked_armor.type == "small" ? 0.135 : 0.23;
// Draw armors
bool is_current_pair = true;
size_t a_n = target_msg.armors_num;
geometry_msgs::msg::Point p_a;
double r = 0;
for (size_t i = 0; i < a_n; i++) {
double tmp_yaw = yaw + i * (2 * M_PI / a_n);
// Only 4 armors has 2 radius and height
if (a_n == 4) {
r = is_current_pair ? r1 : r2;
p_a.z = zc + d_zc + (is_current_pair ? 0 : d_za);
is_current_pair = !is_current_pair;
} else {
r = r1;
p_a.z = zc;
}
p_a.x = xc - r * cos(tmp_yaw);
p_a.y = yc - r * sin(tmp_yaw);
// Hoist loop-invariant conditional outside loop
bool is_small_armor = tracker_->tracked_armor.type == "small";
armors_marker_.scale.y = is_small_armor ? 0.135 : 0.23;
armors_marker_.id = i;
armors_marker_.pose.position = p_a;
// Use cached armor positions from solver instead of recomputing
const auto& armor_positions = solver_->getArmorPositions();
size_t a_n = target_msg.armors_num;
bool is_outpost = (target_msg.id == "outpost");
double outpost_pitch = is_outpost ? -0.2618 : 0.2618;
double two_pi_over_n = 2 * M_PI / a_n;
for (size_t i = 0; i < a_n && i < armor_positions.size(); i++) {
const auto& pos = armor_positions[i];
armors_marker_.id = static_cast<int>(i);
armors_marker_.pose.position.x = pos.x();
armors_marker_.pose.position.y = pos.y();
armors_marker_.pose.position.z = pos.z();
tf2::Quaternion q;
q.setRPY(0, target_msg.id == "outpost" ? -0.2618 : 0.2618, tmp_yaw);
double tmp_yaw = yaw + i * two_pi_over_n;
q.setRPY(0, outpost_pitch, tmp_yaw);
armors_marker_.pose.orientation = tf2::toMsg(q);
marker_array.markers.emplace_back(armors_marker_);
}
selection_marker_.action = visualization_msgs::msg::Marker::ADD;
selection_marker_.points.clear();
selection_marker_.pose.position.y = gimbal_cmd.distance * sin(gimbal_cmd.yaw * M_PI / 180);
selection_marker_.pose.position.x = gimbal_cmd.distance * cos(gimbal_cmd.yaw * M_PI / 180);
selection_marker_.pose.position.z = gimbal_cmd.distance * sin(gimbal_cmd.pitch * M_PI / 180);
// Pre-compute trig values for selection marker
double sin_yaw_cmd = sin(gimbal_cmd.yaw * M_PI / 180);
double cos_yaw_cmd = cos(gimbal_cmd.yaw * M_PI / 180);
double sin_pitch_cmd = sin(gimbal_cmd.pitch * M_PI / 180);
selection_marker_.pose.position.y = gimbal_cmd.distance * sin_yaw_cmd;
selection_marker_.pose.position.x = gimbal_cmd.distance * cos_yaw_cmd;
selection_marker_.pose.position.z = gimbal_cmd.distance * sin_pitch_cmd;
trajectory_marker_.action = visualization_msgs::msg::Marker::ADD;
trajectory_marker_.points.clear();