S2025 planning team b integration

.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,20 @@ 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/
+onedrive_config.json
+zed_sdk.run
+ZED_SDK_Ubuntu20_cuda11.8_v4.0.8.zstd.run
+ros.asc
+
+homework/yolov8n.pt
+homework/yolo11n.pt
+GEMstack/knowledge/detection/yolov8n.pt
+GEMstack/knowledge/detection/yolo11n.pt
+yolov8n.pt
+yolo11n.pt

GEMstack/knowledge/calibration/gem_e4.yaml

@@ -1,7 +1,63 @@
-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_yaw: 0.0 # radians
+T_frontcamera_vehicle:
+- - 0.04979815921110274
+  - -0.02889308605338095
+  - 0.998341277156963
+  - 0.6874899022540225
+- - -0.9986898492450907
+  - 0.010343220052680617
+  - 0.05011489006575621
+  - -0.006750728511462573
+- - -0.011774038000135968
+  - -0.9995289850272167
+  - -0.02834016107658961
+  - -0.07694233627246522
+- - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+T_toplidar_frontcamera:
+- - 0.029096143490519977
+  - -0.9995463314295703
+  - -0.007781115579835407
+  - -0.045768971801312515
+- - -0.005777780139254174
+  - 0.007616104089319731
+  - -0.999954305063568
+  - -0.3868171492765085
+- - 0.9995599190096819
+  - 0.02913977151915098
+  - -0.005553559684576381
+  - -0.6975440651652165
+- - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+T_toplidar_vehicle:
+- - 0.9995177984237671
+  - -0.020904183387756348
+  - 0.022959932684898376
+  - 0.0
+- - 0.020975051447749138
+  - 0.9997758865356445
+  - -0.0028501423075795174
+  - 0.0
+- - -0.0228952094912529
+  - 0.003330353880301118
+  - 0.9997323155403137
+  - 0.33000001311302185
+- - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+date: '2025-02-25 22:31:02.057447'
+calibration_type: absolute_vehicle_calibration
+gnss_location:
+- 1.1
+- 0
+- 1.62
+gnss_yaw: 0.0
+rear_axle_height: 0.33
+reference: rear_axle_center
+vehicle: gem_e4
 top_lidar: !include "gem_e4_ouster.yaml"
 front_camera: !include "gem_e4_oak.yaml"

GEMstack/knowledge/calibration/intrinsic.json

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

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -44,11 +44,23 @@ components:
       inputs: [vehicle, roadgraph, mission]
       outputs: route
   - driving_logic:
-      inputs: all
+      inputs: 
       outputs: intent
+  - parking_component: # one way
+      inputs: all
+      outputs: mission_plan
+  - summon_component: # one way
+      inputs: all
+      outputs: route
+  - route_planning_component: # one way
+      inputs: all
+      outputs: route
   - motion_planning:
       inputs: all
       outputs: trajectory
   - trajectory_tracking:
       inputs: [vehicle, trajectory]
       outputs: 
+  - signaling:
+      inputs: [intent]
+      outputs:

GEMstack/knowledge/defaults/current.yaml

@@ -14,10 +14,10 @@ control:
     pure_pursuit:
         lookahead: 2.0
         lookahead_scale: 3.0
-        crosstrack_gain: 1.0
+        crosstrack_gain: 0.5 # default: 1
         desired_speed: trajectory
     longitudinal_control:
-        pid_p: 1.0
+        pid_p: 1.5
         pid_i: 0.1
         pid_d: 0.0
 

GEMstack/knowledge/vehicle/dynamics.py

@@ -18,6 +18,7 @@ def acceleration_to_pedal_positions(acceleration : float, velocity : float, pitc
     Returns tuple (accelerator_pedal_position, brake_pedal_position, desired_gear)
     """
     model = settings.get('vehicle.dynamics.acceleration_model','hang_v1')
+
     if model == 'hang_v1':
         if gear != 1:
             print("WARNING can't handle gears other than 1 yet")
@@ -36,6 +37,13 @@ def acceleration_to_pedal_positions(acceleration : float, velocity : float, pitc
         else:
             brake_percent = brake_active_range[0] + -(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0])
             throttle_percent = 0
+
+        print("accel : {:.3f}".format(acceleration))
+        print("vel : {:.3f}".format(velocity))
+        print("gear : {}".format(gear))
+        print("gas : {:.2f}".format(throttle_percent))
+        print("brake : {:.2f}".format(brake_percent))
+
         return (max(throttle_percent,0.0),max(brake_percent,0.0),1)
 
     elif model == 'kris_v1':

GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml

@@ -12,7 +12,7 @@ max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear
   - 10000.0
 max_accelerator_power_reverse: 10000.0 #Watts.  Power (backwards) in reverse gear
 
-acceleration_model : kris_v1
+acceleration_model : hang_v1 # kris_v1
 accelerator_active_range : [0.2, 1.0]   #range of accelerator pedal where output acceleration is not flat
 brake_active_range : [0,1]             #range of brake pedal where output deceleration is not flat
 

GEMstack/mathutils/dubins.py

@@ -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/transforms.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]