more fixes for real life testing

Author: mdurrani808

src/simulation/scripts/heading_publisher.py

@@ -4,11 +4,7 @@
 
 import rclpy
 from rclpy.node import Node
-<<<<<<< HEAD
-from rclpy.qos import QoSPresetProfiles
-=======
 from rclpy.qos import SensorDataQoS
->>>>>>> f33dc56 (building)
 from sensor_msgs.msg import MagneticField
 from msgs.msg import Heading
 
@@ -26,69 +22,34 @@ def __init__(self):
         self.heading_acc_ = self.get_parameter('heading_acc').get_parameter_value().double_value
 
         self.pub_ = self.create_publisher(Heading, heading_topic, 10)
-<<<<<<< HEAD
-        
-        # Always cache the first heading to start at 0 degrees
-        self.initial_heading_enu = None
-=======
->>>>>>> f33dc56 (building)
 
         self.sub_ = self.create_subscription(
             MagneticField,
             mag_topic,
             self.on_mag,
-<<<<<<< HEAD
-            QoSPresetProfiles.SENSOR_DATA.value,
-        )
-
-        self.get_logger().info(
-            f'Heading publisher: {mag_topic} -> {heading_topic} (Force Initial Offset: Enabled)'
-=======
             SensorDataQoS(),
         )
 
         self.get_logger().info(
             f'Heading publisher: {mag_topic} -> {heading_topic}'
->>>>>>> f33dc56 (building)
         )
 
     def on_mag(self, msg: MagneticField):
         bx = msg.magnetic_field.x
         by = msg.magnetic_field.y
 
-<<<<<<< HEAD
-        # In Gazebo's magnetometer the field is reported in the robot body frame
-        # with the simulated Earth field pointing North (+Y world).  For a robot
-        # with x=forward, y=left this gives:
-        #   bx = H * sin(yaw_enu)   (North projected onto forward axis)
-        #   by = H * cos(yaw_enu)   (North projected onto left axis)
-        # So atan2(bx, by) = atan2(sin θ, cos θ) = θ, the correct CCW-from-East
-        # ENU heading.  Using atan2(by, bx) would give a CW compass bearing
-        # instead, inverting the sign of every turn.
-        heading_enu_raw = math.degrees(math.atan2(bx, by))
-
-        if self.initial_heading_enu is None:
-            self.initial_heading_enu = heading_enu_raw
-            self.get_logger().info(f'Initial raw heading cached: {self.initial_heading_enu:.2f} deg. Starting at 0.00 deg.')
-        
-        # Apply offset so we start at 0 (East)
-        # ENU heading: degrees CCW from East
-        heading_enu = (heading_enu_raw - self.initial_heading_enu + 360.0) % 360.0
+        # Gazebo magnetometer reports field in robot body frame (x=forward, y=left).
+        # Earth's horizontal field points North, so:
+        #   bx = H*sin(yaw_enu),  by = H*cos(yaw_enu)
+        # atan2(bx, by) = atan2(sin θ, cos θ) = θ = ENU heading directly.
+        heading_enu_deg = math.degrees(math.atan2(bx, by))
 
         # Compass bearing: degrees clockwise from North
-        # North is 90 deg ENU.
-        compass_bearing = (90.0 - heading_enu + 360.0) % 360.0
-=======
-        # compass bearing: degrees clockwise from north
-        compass_bearing = (math.degrees(math.atan2(-by, bx)) + 360.0) % 360.0
-
-        # ENU heading: degrees CCW from east
-        heading_enu = (90.0 - compass_bearing + 360.0) % 360.0
->>>>>>> f33dc56 (building)
+        compass_bearing = (90.0 - heading_enu_deg + 360.0) % 360.0
 
         out = Heading()
         out.header = msg.header
-        out.heading = heading_enu
+        out.heading = heading_enu_deg   # degrees — gps_pose_publisher applies * pi/180
         out.heading_acc = self.heading_acc_
         out.compass_bearing = compass_bearing
         self.pub_.publish(out)

src/subsystems/navigation/mag_heading/mag_heading/mag_heading_node.py

@@ -83,12 +83,18 @@ def _mag_cb(self, msg: MagneticField):
         raw = np.array([msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z])
         corrected = self.soft_iron @ (raw - self.hard_iron)
 
-        # compass_bearing: magnetic north, 0=North CW degrees [0, 360)
-        mag_heading_rad = math.atan2(corrected[1], corrected[0])
+        # Body-frame magnetometer (x=forward, y=left):
+        #   bx = H*sin(yaw_enu),  by = H*cos(yaw_enu)
+        # so atan2(bx, by) = atan2(sin θ, cos θ) = θ = ENU heading directly.
+        # (Same swap used by the simulation heading publisher.)
+        mag_heading_rad = math.atan2(corrected[0], corrected[1])
+
+        # compass_bearing: 0=North CW degrees [0, 360)
         compass_bearing_deg = (90.0 - math.degrees(mag_heading_rad)) % 360.0
 
-        # heading: true north if declination known, magnetic otherwise
-        # ROS convention: 0=East, CCW, radians
+        # heading: true north if declination known, magnetic otherwise.
+        # Published in degrees to match the simulation heading publisher so that
+        # gps_pose_publisher can apply its (* M_PI / 180.0) conversion uniformly.
         if self.declination_rad is not None:
             heading_rad = mag_heading_rad + self.declination_rad
         else:
@@ -98,15 +104,15 @@ def _mag_cb(self, msg: MagneticField):
         cov = msg.magnetic_field_covariance
         b_xy_mag = math.hypot(corrected[0], corrected[1])
         if b_xy_mag > 1e-9 and cov[0] > 0.0:
-            heading_acc = math.sqrt((cov[0] + cov[4]) / 2.0) / b_xy_mag
+            heading_acc_deg = math.degrees(math.sqrt((cov[0] + cov[4]) / 2.0) / b_xy_mag)
         else:
-            heading_acc = 0.0
+            heading_acc_deg = 0.0
 
         out = Heading()
         out.header.stamp = msg.header.stamp
         out.header.frame_id = self.frame_id
-        out.heading = heading_rad
-        out.heading_acc = heading_acc
+        out.heading = math.degrees(heading_rad)   # degrees — matches sim heading publisher
+        out.heading_acc = heading_acc_deg
         out.compass_bearing = compass_bearing_deg
         self.pub.publish(out)
 

umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/metadata.yaml

@@ -0,0 +1,21 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 0
+  starting_time:
+    nanoseconds_since_epoch: 9223372036854775807
+  message_count: 0
+  topics_with_message_count:
+    []
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - rosbag2_1969_12_31-19_14_31_0.db3
+  files:
+    - path: rosbag2_1969_12_31-19_14_31_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 9223372036854775807
+      duration:
+        nanoseconds: 0
+      message_count: 0

umdloop athena-code on_rover rosbag2_1969_12_31-19_14_31/rosbag2_1969_12_31-19_14_31_0.db3

@@ -0,0 +1,21 @@
+rosbag2_bagfile_information:
+  version: 5
+  storage_identifier: sqlite3
+  duration:
+    nanoseconds: 0
+  starting_time:
+    nanoseconds_since_epoch: 9223372036854775807
+  message_count: 0
+  topics_with_message_count:
+    []
+  compression_format: ""
+  compression_mode: ""
+  relative_file_paths:
+    - rosbag2_1969_12_31-19_15_26_0.db3
+  files:
+    - path: rosbag2_1969_12_31-19_15_26_0.db3
+      starting_time:
+        nanoseconds_since_epoch: 9223372036854775807
+      duration:
+        nanoseconds: 0
+      message_count: 0