Fixed orientation

Greenzie · Nov 13, 2023 · 2a407a3 · 2a407a3
1 parent ae959f2
commit 2a407a3
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp
@@ -861,7 +861,7 @@ namespace RobotLocalization
         double enu_heading_rad = nav_pvt_heading_rad;
         if(ned_to_enu_nav_pvt_)
         {
-          enu_heading_rad = PI / 2.0 - nav_pvt_heading_rad;
+          enu_heading_rad = PI - nav_pvt_heading_rad;
         }
         quat.setRPY(0.0, 0.0, enu_heading_rad);
         request.geo_pose.orientation = tf2::toMsg(quat);
@@ -937,7 +937,7 @@ namespace RobotLocalization
         double enu_heading_rad = nav_pvt_heading_rad;
         if(ned_to_enu_nav_pvt_)
         {
-          enu_heading_rad = PI / 2.0 - nav_pvt_heading_rad;
+          enu_heading_rad = PI - nav_pvt_heading_rad;
         }
         orientation_quat.setRPY(0.0, 0.0, enu_heading_rad);