modify tranfer xy to vehicle ax

src/decition/decition_brain_sf/decition/adc_tools/transfer.cpp

@@ -9,19 +9,30 @@ using namespace std;
 
 
 ///大地转车体
+//iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
+//{
+//    double x_vehicle, y_vehicle;
+//    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
+
+//    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
+//    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
+
+//    x_vehicle = -cos(angle) * distance;
+//    y_vehicle = sin(angle) * distance;
+//    return  iv::Point2D(x_vehicle, y_vehicle);
+//}
+
 iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
 {
     double x_vehicle, y_vehicle;
-    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
 
-    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
-    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
+    double x_t= x_path- pos.gps_x;
+    double y_t= y_path- pos.gps_y;
 
-    x_vehicle = -cos(angle) * distance;
-    y_vehicle = sin(angle) * distance;
+    x_vehicle = x_t * cos(pos.ins_heading_angle * PI / 180) - y_t * sin(pos.ins_heading_angle * PI / 180);
+    y_vehicle = x_t * sin(pos.ins_heading_angle * PI / 180) + y_t * cos(pos.ins_heading_angle * PI / 180);
     return  iv::Point2D(x_vehicle, y_vehicle);
 }
-
 ///车体转大地
 iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
 {