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

fix_wust_build

Browse Source
This commit is contained in:
cyy_mac
2026-03-27 04:14:09 +08:00
parent 164cb36c8f
commit d317ee5470
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/rm_auto_aim/armor_solver/src/trajectory_planner.cpp
View File

@@ -30,7 +30,7 @@ GimbalState SegPlanner::plan(const TargetInfo& target, double dt) {
std::vector<GimbalState> states; std::vector<GimbalState> states;
states.reserve(horizon + 1); states.reserve(horizon + 1);
for (int i = 0; i <= horizon; ++i) { for (size_t i = 0; i <= horizon; ++i) {
double t = i * time_step; double t = i * time_step;
auto state = predictTarget(target, t); auto state = predictTarget(target, t);
states.push_back(state); states.push_back(state);
@@ -311,7 +311,7 @@ GimbalState MpcPlanner::plan(const TargetInfo& target, double dt) {
std::vector<GimbalState> ref_traj; std::vector<GimbalState> ref_traj;
ref_traj.reserve(N); ref_traj.reserve(N);
for (int i = 0; i < N; ++i) { for (size_t i = 0; i < static_cast<size_t>(N); ++i) {
double t = i * time_step; double t = i * time_step;
Eigen::Vector3d pred_pos = target.position + t * target.velocity; Eigen::Vector3d pred_pos = target.position + t * target.velocity;
double pred_yaw = target.yaw + t * target.v_yaw; double pred_yaw = target.yaw + t * target.v_yaw;
@@ -331,7 +331,7 @@ GimbalState MpcPlanner::plan(const TargetInfo& target, double dt) {
x_.col(0) << target.yaw, target.v_yaw; x_.col(0) << target.yaw, target.v_yaw;
// 3. Set reference trajectory // 3. Set reference trajectory
for (int i = 0; i < N; ++i) { for (size_t i = 0; i < static_cast<size_t>(N); ++i) {
x_ref_.col(i) << ref_traj[i].yaw.p, ref_traj[i].yaw.v; x_ref_.col(i) << ref_traj[i].yaw.p, ref_traj[i].yaw.v;
} }
@@ -367,7 +367,7 @@ void MpcPlanner::solveMpc(const std::vector<GimbalState>& ref_traj) {
// Simple forward simulation with feedback // Simple forward simulation with feedback
// This is a simplified MPC that uses the reference trajectory // This is a simplified MPC that uses the reference trajectory
// and applies acceleration constraints // and applies acceleration constraints
for (int i = 0; i < N - 1; ++i) { for (size_t i = 0; i < static_cast<size_t>(N - 1); ++i) {
// Get reference state // Get reference state
Eigen::Vector2d x_ref = x_ref_.col(i); Eigen::Vector2d x_ref = x_ref_.col(i);
@@ -388,7 +388,7 @@ void MpcPlanner::solveMpc(const std::vector<GimbalState>& ref_traj) {
// Store solution // Store solution
mpc_solution_yaw_.clear(); mpc_solution_yaw_.clear();
mpc_solution_yaw_.reserve(N); mpc_solution_yaw_.reserve(N);
for (int i = 0; i < N; ++i) { for (size_t i = 0; i < static_cast<size_t>(N); ++i) {
State1D yaw_state(x_(0, i), x_(1, i), 0.0); State1D yaw_state(x_(0, i), x_(1, i), 0.0);
mpc_solution_yaw_.push_back(yaw_state); mpc_solution_yaw_.push_back(yaw_state);
} }