fix yaw

src/rm_auto_aim/armor_solver/src/trajectory_planner.cpp

@@ -234,8 +234,31 @@ SegPlanner::computeArmorAngle(const Eigen::Vector3d& target_pos,
 
     int selected_id = static_cast<int>(temp_angle / (2 * M_PI / armors_num));
 
+    // Compute actual armor yaw angle in world frame
+    double armor_world_yaw = target_yaw + selected_id * (2 * M_PI / armors_num);
+
+    // Determine radius and height offset for this armor
+    bool is_current_pair = true;
+    double r = 0., target_dz = 0.;
+    if (armors_num == 4) {
+        for (int i = 0; i < selected_id; i++) {
+            is_current_pair = !is_current_pair;
+        }
+        r = is_current_pair ? radius_1 : radius_2;
+        target_dz = d_zc + (is_current_pair ? 0 : d_za);
+    } else {
+        r = radius_1;
+        target_dz = d_zc;
+    }
+
+    // Compute actual armor position
+    Eigen::Vector3d armor_pos = target_center + Eigen::Vector3d(
+        -r * std::cos(armor_world_yaw),
+        -r * std::sin(armor_world_yaw),
+        target_dz);
+
     // Compute actual yaw angle to armor
-    double armor_yaw = std::atan2(target_pos.y(), target_pos.x());
+    double armor_yaw = std::atan2(armor_pos.y(), armor_pos.x());
 
     return {armor_yaw, selected_id};
 }