Sr viz main

.gitignore

@@ -32,6 +32,7 @@ share/python-wheels/
 .installed.cfg
 *.egg
 MANIFEST
+.idea/
 
 # PyInstaller
 #  Usually these files are written by a python script from a template
@@ -134,6 +135,7 @@ venv/
 ENV/
 env.bak/
 venv.bak/
+*.DS_Store
 
 # Spyder project settings
 .spyderproject
@@ -165,3 +167,11 @@ 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/
+
+.vscode/
+setup/zed_sdk.run
+
+cuda/
+#Ignore ROS bags
+*.bag
+

GEMstack/knowledge/calibration/gem_e4.yaml

@@ -1,6 +1,6 @@
-calibration_date: "2024-03-05"  # Date of calibration YYYY-MM-DD
+calibration_date: "2025-02-25"  # 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
+rear_axle_height: 0.2794      # 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
 top_lidar: !include "gem_e4_ouster.yaml"

GEMstack/knowledge/calibration/gem_e4_lidar_cam.yaml

@@ -0,0 +1,9 @@
+# Calibration for top lidar->front rgb camera on GEM e4
+# Calibration Date: 02/25/2025 05:09
+
+lidar_to_camera: [
+                  [ 2.89748006e-02, -9.99580136e-01,  3.68439439e-05, -3.07300513e-02],
+                  [-9.49930618e-03, -3.12215512e-04, -9.99954834e-01, -3.86689354e-01],
+                  [ 9.99534999e-01,  2.89731321e-02, -9.50437214e-03, -6.71425124e-01],
+                  [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]
+                ]

GEMstack/knowledge/calibration/gem_e4_oak.yaml

@@ -1,3 +1,8 @@
+# Calibration for front rgb camera->vehicle on GEM e4
+# Calibration Date: 02/25/2025 05:09
+
 reference: rear_axle_center  # rear axle center
-rotation: [[0,0,1],[-1,0,0],[0,-1,0]] # rotation matrix mapping z to forward, x to left, y to down, guesstimated
-center_position: [1.78,0,1.58] # meters, center camera, guesstimated
+rotation: [[ 0.00349517, -0.03239524, 0.99946903], # rotation component of camera -> vehicle matrix
+          [-0.99996547,  0.00742285,  0.0037375],
+          [-0.00753999, -0.99944757, -0.03236817]] # rotation matrix mapping z to forward, x to left, y to down, guesstimated
+center_position: [ 1.75864913, 0.01238124, 1.54408419] # translation component of camera -> vehicle matrix (in meters)

GEMstack/knowledge/calibration/gem_e4_ouster.yaml

@@ -1,3 +1,8 @@
+# Calibration for top lidar->vehicle on GEM e4
+# Calibration Date: 02/25/2025 05:09
+
 reference: rear_axle_center  # rear axle center
-position: [1.10,0,2.03] # meters, calibrated by Hang's watchful eye
-rotation: [[1,0,0],[0,1,0],[0,0,1]]  #rotation matrix mapping lidar frame to vehicle frame
+position: [1.1, 0.03773044170906172, 1.9525244316515322] # top lidar to vehicle matrix translation component
+rotation: [[ 0.99941328,  0.02547416,  0.02289458], # top lidar to vehicle matrix rotation component
+       [-0.02530855,  0.99965159, -0.00749488],
+       [-0.02307753,  0.00691106,  0.99970979]]

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -31,7 +31,7 @@ components:
       inputs: [vehicle, roadgraph, agents]
       outputs: agent_intents
   - relations_estimation:
-      inputs: [vehicle, roadgraph, agents, obstacles]
+      inputs: all
       outputs: relations
   - predicate_evaluation:
       inputs: [vehicle, roadgraph, agents, obstacles]
@@ -52,3 +52,6 @@ components:
   - trajectory_tracking:
       inputs: [vehicle, trajectory]
       outputs: 
+  - signaling:
+      inputs: [intent] 
+      outputs: 

GEMstack/knowledge/defaults/current.yaml

@@ -13,22 +13,34 @@ control:
         brake_speed : 2.0
     pure_pursuit:
         lookahead: 2.0
-        lookahead_scale: 3.0
-        crosstrack_gain: 1.0
+        lookahead_scale: 2.0
+        crosstrack_gain: 0.3
         desired_speed: trajectory
     longitudinal_control:
-        pid_p: 1.0
-        pid_i: 0.1
+        pid_p: 0.8
+        pid_i: 0.03
         pid_d: 0.0
+planning:
+    longitudinal_plan:
+        mode:               'real' # 'real' or 'sim'
+        yielder:            'expert' # 'expert', 'analytic', or 'simulation'
+        planner:            'dt' # 'milestone', 'dt', or 'dx'
+        desired_speed:      1.0
+        acceleration:       0.5
+        max_deceleration:   6.0
+        deceleration:       2.0
+        min_deceleration:   0.5
+        yield_deceleration: 0.5
+
 
 #configure the simulator, if using
 simulator:
     dt: 0.01
     real_time_multiplier: 1.0    # make the simulator run faster than real time by making this > 1
     gnss_emulator: 
-        dt: 0.1    #10Hz
+        dt: 0.05    #10Hz
         #position_noise: 0.1  #10cm noise
         #orientation_noise: 0.04  #2.3 degrees noise
         #velocity_noise:
         #    constant: 0.04  #4cm/s noise
-        #    linear: 0.02    #2% noise
+        #    linear: 0.02    #2% noise

GEMstack/knowledge/vehicle/dynamics.py

@@ -31,13 +31,63 @@ def acceleration_to_pedal_positions(acceleration : float, velocity : float, pitc
             if acceleration < -dry_decel*0.5 or (acceleration <= 0 and velocity < 0.1):  # a little deadband to avoid oscillation
                 throttle_percent = 0.0  #drift to stop
             else:
-                throttle_percent = accel_active_range[0] + (acceleration+dry_decel)/max_accel * (accel_active_range[1]-accel_active_range[0])
+                throttle_percent = accel_active_range[0] + ((acceleration+dry_decel)/max_accel * (accel_active_range[1]-accel_active_range[0]))
             brake_percent = 0
         else:
-            brake_percent = brake_active_range[0] + -(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0])
+            brake_percent = brake_active_range[0] + (-(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0]))
             throttle_percent = 0
+        print(acceleration, (max(throttle_percent,0.0),max(brake_percent,0.0),1))
         return (max(throttle_percent,0.0),max(brake_percent,0.0),1)
 
+
+    # Model that's built on top of Hang's model.
+    # Designed to allow for various scaling based on current vehicle velocity, and use of functions other than linear
+    # for scaling. 
+    # DANGER: NEEDS TESTED
+    elif model == 'avery_v1':
+        model = settings.get('vehicle.dynamics.acceleration_model', 'avery_v1')
+        if gear != 1:
+            print("WARNING: Can't handle gears other than 1 yet")
+
+        max_accel = settings.get('vehicle.dynamics.max_accelerator_acceleration')[1]  # m/s^2
+        max_brake = settings.get('vehicle.dynamics.max_brake_deceleration')  # m/s^2
+        dry_decel = settings.get('vehicle.dynamics.internal_dry_deceleration')  # m/s^2
+        accel_active_range = settings.get('vehicle.dynamics.accelerator_active_range')  # pedal position fraction
+        brake_active_range = settings.get('vehicle.dynamics.brake_active_range')  # pedal position fraction
+        mapping_function = settings.get('vehicle.dynamics.pedal_mapping_function', 'linear')
+        velocity_scalar = settings.get('vehicle.dynamics.velocity_scaling_factor', 0)  # Adjust sensitivity based on velocity
+
+
+        def apply_function(value, function='linear'):
+            if function == 'linear':
+                value = value # already linear mapping
+
+            # no guarantee that these functions will give good results until we get more test data about accel -> pedal mapping.
+            elif function == 'quadratic':
+                value = value ** 2 # value squared
+            elif function == 'exponential':
+                value = (math.exp(value) - 1) / 2
+            elif function == 'log':
+                value = math.sqrt(math.log(value + 1)) + 0.2
+
+            value += velocity * velocity_scalar
+            return value
+
+        if acceleration > -dry_decel:
+            if acceleration < -dry_decel*0.5 or (acceleration <= 0 and velocity < 0.1):  # a little deadband to avoid oscillation
+                throttle_percent = 0.0  #drift to stop
+            else:
+                throttle_percent = accel_active_range[0] + ((acceleration+dry_decel)/max_accel * (accel_active_range[1]-accel_active_range[0]))
+                throttle_percent = apply_function(throttle_percent, mapping_function)
+            brake_percent = 0
+        else:
+            brake_percent = brake_active_range[0] + (-(acceleration+dry_decel)/max_brake * (brake_active_range[1]-brake_active_range[0]))
+            brake_percent = apply_function(brake_percent, mapping_function)
+            throttle_percent = 0
+        print(acceleration, (max(throttle_percent,0.0),max(brake_percent,0.0),1))
+        return (max(throttle_percent,0.0),max(brake_percent,0.0),1)
+
+
     elif model == 'kris_v1':
         brake_max = settings.get('vehicle.dynamics.max_brake_deceleration')
         reverse_accel_max = settings.get('vehicle.dynamics.max_accelerator_acceleration_reverse')

GEMstack/knowledge/vehicle/gem_e2_dynamics.yaml

@@ -14,7 +14,7 @@ max_accelerator_power_reverse: 10000.0 #Watts.  Power (backwards) in reverse gea
 
 acceleration_model : kris_v1
 accelerator_active_range : [0.32, 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
+brake_active_range : [0.25,1]             #range of brake pedal where output deceleration is not flat
 
 internal_dry_deceleration: 0.2      #m/s^2: deceleration due to internal dry friction (non-speed dependent)
 internal_viscous_deceleration: 0.05  #1/s: scales the current velocity to get deceleration due to internal viscous friction (speed dependent)

GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml

@@ -1,22 +1,24 @@
-mass: 300.0  #kg
+mass: 700.0  #kg
 gravity: 9.81 #m/s^2
-longitudinal_friction : 1.0  # unitless
-lateral_friction : 1.0       # unitless
-max_brake_deceleration: 8.0  #m/s^2.  Deceleration at max brake pedal
-max_accelerator_acceleration: #m/s^2. Acceleration at max accelerator pedal, by gear
+longitudinal_friction : 1.0  # unitless TODO find make/model of tires 
+lateral_friction : 1.0       # unitless (I will bet these things do not have 1 coefficent of fricition)
+max_brake_deceleration: 8.0  #m/s^2.  Deceleration at max brake pedal TODO (un cap this cap)
+max_accelerator_acceleration: #m/s^2. Acceleration at max accelerator pedal, by gear (un cap this cap)
   - 0.0
   - 5.0
 max_accelerator_acceleration_reverse: 2.5 #m/s^2. Acceleration (backwards) in reverse gear
-max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear
+max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear (updated from website)
   - 0.0
-  - 10000.0
-max_accelerator_power_reverse: 10000.0 #Watts.  Power (backwards) in reverse gear
+  - 5000.0
+max_accelerator_power_reverse: 5000.0 #Watts.  Power (backwards) in reverse gear
 
-acceleration_model : kris_v1
+acceleration_model : hang_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
+brake_active_range : [0.5,1]             #range of brake pedal where output deceleration is not flat
 
 internal_dry_deceleration: 0.2      #m/s^2: deceleration due to internal dry friction (non-speed dependent)
 internal_viscous_deceleration: 0.05  #1/s: scales the current velocity to get deceleration due to internal viscous friction (speed dependent)
 aerodynamic_drag_coefficient: 0.01  #units in s, scaled by velocity^2 to get deceleration due to aerodynamic drag
 acceleration_deadband: 0.1          #m/s^2: minimum acceleration to be considered non-zero
+velocity_scaling_factor: 0.0           # Velocity * this scalar will be added to brake and throttle percent. 
+pedal_mapping_function: linear

GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml

@@ -3,4 +3,4 @@ ros_topics:
   front_camera: /oak/rgb/image_raw
   front_depth: /oak/stereo/image_raw
   gnss: /septentrio_gnss/insnavgeod
-  
+

GEMstack/mathutils/quadratic_equation.py

@@ -0,0 +1,21 @@
+import math
+
+def quad_root(a : float, b : float, c : float) -> float:
+    """Calculates the roots of a quadratic equation
+
+    Args:
+        a (float): coefficient of x^2
+        b (float): coefficient of x
+        c (float): constant term
+
+    Returns:
+        float: returns all valid roots of the quadratic equation
+    """
+    x1 = (-b + max(0,(b**2 - 4*a*c))**0.5)/(2*a)
+    x2 = (-b - max(0,(b**2 - 4*a*c))**0.5)/(2*a)
+
+    if math.isnan(x1): x1 = 0
+    if math.isnan(x2): x2 = 0
+
+    valid_roots = [n for n in [x1, x2] if not type(n) is complex]
+    return valid_roots