subscribes to combined gnss data

Author: pravshot

GEMstack/knowledge/vehicle/gem_e2_sensor_environment_gazebo.yaml

@@ -2,6 +2,5 @@ ros_topics:
   top_lidar: /velodyne_points
   front_camera: /front_single_camera/image_raw
   front_depth: /notopic
-  gps: /gps/fix
-  imu: /imu
+  gnss: /insnavgeod
 

GEMstack/knowledge/vehicle/gem_e4_sensor_environment_gazebo.yaml

@@ -2,6 +2,5 @@ ros_topics:
   top_lidar: /velodyne_points
   front_camera: /front_single_camera/image_raw
   front_depth: /notopic
-  gps: /gps/fix
-  imu: /imu
+  gnss: /insnavgeod
 

GEMstack/onboard/interface/gem_gazebo.py

@@ -6,6 +6,7 @@
 # ROS Headers
 import rospy
 from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
+from septentrio_gnss_driver.msg import INSNavGeod
 from geometry_msgs.msg import Vector3Stamped
 from sensor_msgs.msg import JointState  # For reading joint states from Gazebo
 # Changed from AckermannDriveStamped
@@ -56,10 +57,6 @@ def __init__(self):
 
 
 
-        # IMU data subscriber
-        self.imu_sub = None
-        self.imu_data = None
-
         # GNSS data subscriber
         self.gnss_sub = None
 
@@ -77,12 +74,6 @@ def __init__(self):
         self.ackermann_cmd = AckermannDrive()
         self.last_command = None  # Store the last command
 
-        # Subscribe to IMU topic by default
-        self.imu_sub = rospy.Subscriber("/imu", Imu, self.imu_callback)
-
-        # Subscribe to GNSS Velocitu
-        self.gnss_vel_sub = rospy.Subscriber("/gps/fix_velocity", Vector3Stamped, self.gnss_vel_callback)
-
         # Add clock subscription for simulation time
         self.sim_time = rospy.Time(0)
         self.clock_sub = rospy.Subscriber('/clock', Clock, self.clock_callback)
@@ -97,9 +88,6 @@ def time(self):
         # Return Gazebo simulation time
         return self.sim_time.to_sec()
 
-    def imu_callback(self, msg: Imu):
-        self.imu_data = msg
-
     def gnss_vel_callback(self, msg):
         self.last_reading.speed = np.linalg.norm([msg.vector.x, msg.vector.y] )
 
@@ -109,21 +97,11 @@ def get_reading(self) -> GEMVehicleReading:
 
     def subscribe_sensor(self, name, callback, type=None):
         if name == 'gnss':
-            topic = self.ros_sensor_topics['gps']
-            # Fuse IMU orientation with GNSS position
-            def gnss_callback_wrapper(gps_msg: NavSatFix):
-                if self.imu_data is None:
-                    return  # Wait for IMU data
-
-                # Get orientation from IMU
-                quaternion = (
-                    self.imu_data.orientation.x,
-                    self.imu_data.orientation.y,
-                    self.imu_data.orientation.z,
-                    self.imu_data.orientation.w
-                )
-                print(f"[IMU] Orientation: {quaternion}")
-                roll, pitch, yaw = euler_from_quaternion(quaternion)
+            topic = self.ros_sensor_topics['gnss']
+            def gnss_callback_wrapper(gnss_msg: INSNavGeod):
+                roll, pitch, yaw = gnss_msg.roll, gnss_msg.pitch, gnss_msg.heading
+                # Convert from degrees to radians
+                roll, pitch, yaw = math.radians(roll), math.radians(pitch), math.radians(yaw)
 
                 # Transform yaw to correct frame - Gazebo typically uses ROS standard frame (x-forward)
                 # while navigation uses x-east reference frame
@@ -140,10 +118,10 @@ def gnss_callback_wrapper(gps_msg: NavSatFix):
                 # Create fused pose with transformed yaw
                 pose = ObjectPose(
                     frame=ObjectFrameEnum.GLOBAL,
-                    t=gps_msg.header.stamp,
-                    x=gps_msg.longitude,
-                    y=gps_msg.latitude,
-                    z=gps_msg.altitude,
+                    t=gnss_msg.header.stamp,
+                    x=gnss_msg.longitude,
+                    y=gnss_msg.latitude,
+                    z=gnss_msg.height,
                     roll=roll,
                     pitch=pitch,
                     yaw=navigation_yaw
@@ -152,12 +130,12 @@ def gnss_callback_wrapper(gps_msg: NavSatFix):
                 # Create GNSS reading with fused data
                 reading = GNSSReading(
                     pose=pose,
-                    speed= self.last_reading.speed,
-                    status='FIX' if gps_msg.status.status >= 0 else 'NO_FIX'
+                    speed=np.linalg.norm([gnss_msg.ve, gnss_msg.vn]),
+                    status='error' if gnss_msg.error else 'ok'
                 )
                 # Added debug
                 print(
-                    f"[GNSS] Raw coordinates: Lat={gps_msg.latitude:.6f}, Lon={gps_msg.longitude:.6f}")
+                    f"[GNSS] Raw coordinates: Lat={gnss_msg.latitude:.6f}, Lon={gnss_msg.longitude:.6f}")
                 # Added debug
                 print(
                     f"[GNSS-FUSED] Orientation: Roll={roll:.2f}, Pitch={pitch:.2f}, Yaw={yaw:.2f} rad")
@@ -166,7 +144,7 @@ def gnss_callback_wrapper(gps_msg: NavSatFix):
 
                 callback(reading)
 
-            self.gnss_sub = rospy.Subscriber(topic, NavSatFix, gnss_callback_wrapper)
+            self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, gnss_callback_wrapper)
 
         elif name == 'top_lidar':
             topic = self.ros_sensor_topics[name]