Perceptionb working gnss -> main

GEMstack/knowledge/calibration/gem_e4.yaml

@@ -1,7 +1,7 @@
 calibration_date: "2024-03-05"  # Date of calibration YYYY-MM-DD
 reference: rear_axle_center  # rear axle center
 rear_axle_height: 0.33      # height of rear axle center above flat ground
-gnss_location: [1.10,0.1.62] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py.  Note conflict with pure pursuit location?
+gnss_location: [1.10,0,1.62] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py.  Note conflict with pure pursuit location?
 gnss_yaw: 0.0 # radians
 top_lidar: !include "gem_e4_ouster.yaml"
 front_camera: !include "gem_e4_oak.yaml"

GEMstack/onboard/interface/gem_hardware.py

@@ -6,6 +6,7 @@
 import rospy
 from std_msgs.msg import String, Bool, Float32, Float64
 from sensor_msgs.msg import Image,PointCloud2
+import message_filters
 try:
     from novatel_gps_msgs.msg import NovatelPosition, NovatelXYZ, Inspva
 except ImportError:
@@ -55,6 +56,7 @@ def __init__(self):
         self.front_depth_sub = None
         self.top_lidar_sub = None
         self.stereo_sub = None
+        self.sync = None
         self.faults = []
 
         # -------------------- PACMod setup --------------------
@@ -177,6 +179,7 @@ def callback_with_gnss_reading(inspva_msg: Inspva):
                 else:
                     def callback_with_gnss_reading(msg: INSNavGeod):
                         pose = ObjectPose(ObjectFrameEnum.GLOBAL,
+                                    t = 0,
                                     x=msg.longitude,
                                     y=msg.latitude,
                                     z=msg.height,
@@ -186,7 +189,7 @@ def callback_with_gnss_reading(msg: INSNavGeod):
                                     )
                         speed = np.sqrt(msg.ve**2 + msg.vn**2)
                         callback(GNSSReading(pose,speed,('error' if msg.error else 'ok')))
-                    self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading)
+                    self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading)
         elif name == 'top_lidar':
             topic = self.ros_sensor_topics[name]
             if type is not None and (type is not PointCloud2 and type is not np.ndarray):
@@ -227,6 +230,40 @@ def callback_with_cv2(msg : Image):
                     cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough")
                     callback(cv_image)
                 self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
+        elif name == 'sensor_fusion_Lidar_Camera_GNSS':
+            def callback_with_cv2_numpy_gnss(rgb_image_msg: Image, lidar_pc2_msg: PointCloud2, gnss_msg: INSNavGeod):
+                points = conversions.ros_PointCloud2_to_numpy(lidar_pc2_msg, want_rgb=False)
+                cv_image = conversions.ros_Image_to_cv2(rgb_image_msg, desired_encoding="bgr8")
+                pose = ObjectPose(ObjectFrameEnum.GLOBAL,
+                                    t = 0,
+                                    x=gnss_msg.longitude,
+                                    y=gnss_msg.latitude,
+                                    z=gnss_msg.height,
+                                    yaw=math.radians(gnss_msg.heading),  #heading from north in degrees (TODO: maybe?? check this)
+                                    roll=math.radians(gnss_msg.roll),
+                                    pitch=math.radians(gnss_msg.pitch),
+                                    )
+                speed = np.sqrt(gnss_msg.ve**2 + gnss_msg.vn**2)
+                callback(cv_image,points,GNSSReading(pose,speed,('error' if gnss_msg.error else 'ok')))
+            topic_camera = self.ros_sensor_topics['front_camera']
+            topic_lidar = self.ros_sensor_topics['top_lidar']
+            topic_gnss = self.ros_sensor_topics['gnss']
+            self.front_camera_sub = message_filters.Subscriber(topic_camera, Image)
+            self.top_lidar_sub = message_filters.Subscriber(topic_lidar, PointCloud2)
+            self.gnss_sub = message_filters.Subscriber(topic_gnss, INSNavGeod)
+            self.sync = message_filters.ApproximateTimeSynchronizer([self.front_camera_sub, self.top_lidar_sub,self.gnss_sub], queue_size=10, slop=0.1)
+            self.sync.registerCallback(callback_with_cv2_numpy_gnss)
+        elif name == 'sensor_fusion_Lidar_Camera':
+            def callback_with_cv2_numpy(rgb_image_msg: Image, lidar_pc2_msg: PointCloud2):
+                points = conversions.ros_PointCloud2_to_numpy(lidar_pc2_msg, want_rgb=False)
+                cv_image = conversions.ros_Image_to_cv2(rgb_image_msg, desired_encoding="bgr8")
+                callback(cv_image,points)
+            topic_camera = self.ros_sensor_topics['front_camera']
+            topic_lidar = self.ros_sensor_topics['top_lidar']
+            self.front_camera_sub = message_filters.Subscriber(topic_camera, Image)
+            self.top_lidar_sub = message_filters.Subscriber(topic_lidar, PointCloud2)
+            self.sync = message_filters.ApproximateTimeSynchronizer([self.front_camera_sub, self.top_lidar_sub], queue_size=10, slop=0.1)
+            self.sync.registerCallback(callback_with_cv2_numpy)
 
 
     # PACMod enable callback function