fix_wust

src/rm_auto_aim/armor_solver/include/armor_solver/trajectory_planner.hpp

@@ -11,6 +11,7 @@
 #include <cmath>
 #include <Eigen/Dense>
 #include <angles/angles.h>
+#include <rm_interfaces/msg/target.hpp>
 
 namespace fyt::auto_aim {
 
@@ -331,12 +332,11 @@ private:
     computeNodeStates(const std::vector<GimbalState>& states,
                       const std::vector<double>& dt_vec) const;
 
-    // Build limited quintic segments
-    Trajectory<QuinticSegment>
-    buildLimit(const std::vector<State1D>& yaw_nodes,
-               const std::vector<State1D>& pitch_nodes,
-               double max_yaw_acc,
-               double max_pitch_acc) const;
+    // Build limited quintic segments (currently unused, kept for future)
+    void buildLimit(const std::vector<State1D>& yaw_nodes,
+                    const std::vector<State1D>& pitch_nodes,
+                    double max_yaw_acc,
+                    double max_pitch_acc) const;
 
     // Convert gimbal angle to target angle (accounting for armor offset)
     static std::pair<double, int>

src/rm_auto_aim/armor_solver/src/trajectory_planner.cpp

@@ -13,7 +13,7 @@ namespace fyt::auto_aim {
 // ============================================================================
 
 GimbalState SegPlanner::plan(const TargetInfo& target, double dt) {
-    int horizon = params_.sample_horizon;
+    size_t horizon = static_cast<size_t>(params_.sample_horizon);
     double total_time = params_.sample_total_time;
     double time_step = total_time / horizon;
 
@@ -194,28 +194,17 @@ SegPlanner::computeNodeStates(const std::vector<GimbalState>& states,
     return {yaw_nodes, pitch_nodes};
 }
 
-Trajectory<QuinticSegment>
+void
 SegPlanner::buildLimit(const std::vector<State1D>& yaw_nodes,
                         const std::vector<State1D>& pitch_nodes,
                         double max_yaw_acc,
                         double max_pitch_acc) const {
-    using namespace Eigen;
-
-    Trajectory<QuinticSegment> traj;
-    size_t n = yaw_nodes.size();
-
-    if (n <= 1) return traj;
-
-    traj.reserve(n);
-
-    for (size_t i = 0; i < n - 1; ++i) {
-        // Build yaw segment
-        QuinticSegment seg(1.0);
-        seg = QuinticSegment::build(yaw_nodes[i], yaw_nodes[i + 1], 1.0);
-        traj.push_back(seg, 1.0);
-    }
-
-    return traj;
+    // Currently unused - quintic segments are built directly in plan()
+    // This function is kept for future extension
+    (void)yaw_nodes;
+    (void)pitch_nodes;
+    (void)max_yaw_acc;
+    (void)max_pitch_acc;
 }
 
 std::pair<double, int>
@@ -414,8 +403,18 @@ GimbalState MpcPlanner::getStateAtTime(double t, const std::vector<GimbalState>&
     double total_time = params_.sample_total_time;
     double time_step = total_time / N;
 
-    if (t <= 0) return mpc_solution_yaw_.front();
-    if (t >= total_time) return mpc_solution_yaw_.back();
+    if (t <= 0) {
+        State1D yaw_state = mpc_solution_yaw_.front();
+        State1D pitch_state(ref_traj.front().pitch.p, 0.0, 0.0);
+        return GimbalState(yaw_state.p, yaw_state.v, yaw_state.a,
+                          pitch_state.p, pitch_state.v, pitch_state.a);
+    }
+    if (t >= total_time) {
+        State1D yaw_state = mpc_solution_yaw_.back();
+        State1D pitch_state(ref_traj.back().pitch.p, 0.0, 0.0);
+        return GimbalState(yaw_state.p, yaw_state.v, yaw_state.a,
+                          pitch_state.p, pitch_state.v, pitch_state.a);
+    }
 
     // Find the segment
     int idx = static_cast<int>(t / time_step);