Merge branch 's2025_teamB' into s2025_groupB_state_est

Author: muralikarteek7

.github/workflows/python-app.yml

@@ -0,0 +1,62 @@
+# This workflow will install Python dependencies, run tests and lint with a single version of Python
+# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python
+
+name: Python application
+
+on:
+  push:
+    branches: 
+    - '**'
+
+
+permissions:
+  contents: read
+
+jobs:
+  PEP-Guidelines:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v3
+    - name: Set up Python 3.10
+      uses: actions/setup-python@v3
+      with:
+        python-version: "3.10"
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install flake8 flake8-docstrings pep8-naming
+    - name: Lint with flake8
+      run: |
+        # stop the build if there are Python syntax errors or undefined names
+        flake8 ./GEMstack --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=__init__.py || exit 1
+        # to enable more advanced checks on the repo, uncomment the lines below (There are around 3000 violations)
+        # flake8 ./GEMstack --ignore=D,C901,E402,E231 --count --max-complexity=10 --max-line-length=127 --statistics --exclude=__init__.py || exit 1
+        # if we want to enable documentation checks, uncomment the line below
+        # flake8 ./GEMstack --ignore=E128,E402,E501,F401 --docstring-convention pep257 --max-line-length=120 --exclude=__init__.py || exit 1
+      continue-on-error: false
+
+  Documentation:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v3
+    - name: Set up Python 3.10
+      uses: actions/setup-python@v3
+      with:
+        python-version: "3.10"
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install sphinx sphinx-rtd-theme
+    - name: Generate Documentation
+      run: |
+        # stop the build if there are Python syntax errors or undefined names
+        sphinx-build -b html docs docs/build
+    - name: Save Documentation as Artifact
+      uses: actions/upload-artifact@v4
+      with:
+        name: documentation
+        path: docs/build

.gitignore

@@ -165,3 +165,15 @@ cython_debug/
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
 #.idea/
+
+# ZED run files
+**/*.run
+.vscode/
+setup/zed_sdk.run
+cuda/
+homework/yolov8n.pt
+homework/yolo11n.pt
+GEMstack/knowledge/detection/yolov8n.pt
+GEMstack/knowledge/detection/yolo11n.pt
+yolov8n.pt
+yolo11n.pt

GEMstack/knowledge/calibration/extrinsics.npz

@@ -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/knowledge/calibration/gem_e4.yaml

@@ -0,0 +1,7 @@
+{
+    "fx": 684.8333129882812,
+    "fy": 684.6096801757812,
+    "cx": 573.37109375,
+    "cy": 363.700927734375
+  }
+  

GEMstack/knowledge/calibration/intrinsic.json

@@ -44,7 +44,7 @@ def derivative(self,x,u):
         right = [-fwd[1],fwd[0]]
         phi = u[1]
         d = u[0]
-        return np.array([fwd[0]*d,fwd[1]*d,phi])
+        return np.array([fwd[0]*d,fwd[1]*d,phi*d])
 
 
 class DubinsCarIntegrator(ControlSpace):

GEMstack/mathutils/dubins.py

@@ -36,6 +36,14 @@ def vector_dist(v1, v2) -> float:
     """Euclidean distance between two vectors"""
     return vo.distance(v1,v2)
 
+def vector_dot(v1, v2) -> float:
+    """Dot product between two vectors"""
+    return vo.dot(v1,v2)
+
+def vector_cross(v1, v2) -> float:
+    """Cross product between two 2D vectors"""
+    return vo.cross(v1,v2)
+
 def vector2_angle(v1, v2 = None) -> float:
     """find the ccw angle bewtween two 2d vectors"""
     if v2 is None:
@@ -123,3 +131,16 @@ def xy_to_lat_lon(x_east : float, y_north : float, lat_reference : float, lon_re
     # convert GNSS waypoints into local fixed frame reprented in x and y
     lat, lon = axy.xy2ll(x_east, y_north, lat_reference, lon_reference)
     return lat, lon
+
+def quaternion_to_euler(x : float, y : float, z : float, w : float):
+    t0 = +2.0 * (w * x + y * z)
+    t1 = +1.0 - 2.0 * (x * x + y * y)
+    roll = np.arctan2(t0, t1)
+    t2 = +2.0 * (w * y - z * x)
+    t2 = +1.0 if t2 > +1.0 else t2
+    t2 = -1.0 if t2 < -1.0 else t2
+    pitch = np.arcsin(t2)
+    t3 = +2.0 * (w * z + x * y)
+    t4 = +1.0 - 2.0 * (y * y + z * z)
+    yaw = np.arctan2(t3, t4)
+    return [roll, pitch, yaw]

GEMstack/mathutils/transforms.py

@@ -1,11 +1,13 @@
 from .gem import *
 from ...utils import settings
 import math
+import time
 
 # ROS Headers
 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 +57,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 --------------------
@@ -151,6 +154,7 @@ def subscribe_sensor(self, name, callback, type = None):
         if name == 'gnss':
             topic = self.ros_sensor_topics[name]
             if topic.endswith('inspva'):
+                #GEM e2 uses Novatel GNSS
                 if type is not None and (type is not Inspva and type is not GNSSReading):
                     raise ValueError("GEMHardwareInterface GEM e2 only supports Inspva/GNSSReading for GNSS")
                 if type is Inspva:
@@ -169,24 +173,25 @@ def callback_with_gnss_reading(inspva_msg: Inspva):
                         callback(GNSSReading(pose,speed,inspva_msg.status))
                     self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading)
             else:
-                #assume it's septentrio
+                #assume it's septentrio on GEM e4
                 if type is not None and (type is not INSNavGeod and type is not GNSSReading):
                     raise ValueError("GEMHardwareInterface GEM e4 only supports INSNavGeod/GNSSReading for GNSS")
                 if type is INSNavGeod:
                     self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback)
                 else:
                     def callback_with_gnss_reading(msg: INSNavGeod):
                         pose = ObjectPose(ObjectFrameEnum.GLOBAL,
-                                    x=msg.longitude,
-                                    y=msg.latitude,
+                                    t=self.time(),
+                                    x=math.degrees(msg.longitude),   #Septentrio GNSS uses radians rather than degrees
+                                    y=math.degrees(msg.latitude),
                                     z=msg.height,
                                     yaw=math.radians(msg.heading),  #heading from north in degrees (TODO: maybe?? check this)
                                     roll=math.radians(msg.roll),
                                     pitch=math.radians(msg.pitch),
                                     )
                         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 +232,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

GEMstack/onboard/interface/gem_hardware.py

@@ -0,0 +1,12 @@
+class IdTracker():
+    """Abstracts out id tracking
+    """
+    def __init__(self):
+        self.__id = 0
+
+    def get_new_id(self) -> int:
+        """Returns a unique agent id
+        """
+        assigned_id = self.__id
+        self.__id += 1 # id will intentionally overflow to get back to 0
+        return assigned_id

GEMstack/onboard/perception/IdTracker.py

@@ -0,0 +1,7 @@
+{
+    "fx": 684.8333129882812,
+    "fy": 684.6096801757812,
+    "cx": 573.37109375,
+    "cy": 363.700927734375
+  }
+