Updated Heading and Orientation Calculations to be Consistent with ROS 1 branch (#216)

Author: JordanSlater

ublox_gps/src/hp_pos_rec_product.cpp

@@ -54,8 +54,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9
     imu_.angular_velocity_covariance[0] = -1;
 
     // Transform angle since ublox is representing heading as NED but ROS uses ENU as convention (REP-103).
-    // Alos convert the base-to-rover angle to a robot-to-base angle (consistent with frame_id).
-    double heading = (static_cast<double>(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI) - M_PI_2;
+    double heading = M_PI_2 - (static_cast<double>(m.rel_pos_heading) * 1e-5 / 180.0 * M_PI);
     tf2::Quaternion orientation;
     orientation.setRPY(0, 0, heading);
     imu_.orientation.x = orientation[0];
@@ -67,7 +66,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::msg::NavRELPOSNED9
     imu_.orientation_covariance[8] = 1000.0;
     // When heading is reported to be valid, use accuracy reported in 1e-5 deg units
     if (m.flags & ublox_msgs::msg::NavRELPOSNED9::FLAGS_REL_POS_HEAD_VALID) {
-      imu_.orientation_covariance[8] = ::pow(m.acc_heading / 10000.0, 2);
+      imu_.orientation_covariance[8] = ::pow(m.acc_heading * 1e-5 / 180.0 * M_PI, 2);
     }
 
     imu_pub_->publish(imu_);