S2025 Perception Merge

.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
@@ -168,4 +170,15 @@ cython_debug/
 
 .vscode/
 setup/zed_sdk.run
-cuda/
+
+cuda/
+#Ignore ROS bags
+*.bag
+
+*.png
+*.npz
+/4.21
+/4.30
+/5.9
+/parking_data
+/perception_4.9

GEMstack/knowledge/calibration/cameras.yaml

@@ -0,0 +1,35 @@
+cameras:
+   front:
+     K:
+       - [684.83331299, 0.0,          573.37109375]
+       - [0.0,          684.60968018, 363.70092773]
+       - [0.0,          0.0,          1.0]
+     D: [0.0, 0.0, 0.0, 0.0, 0.0]
+     T_l2c:
+       - [ 0.0289748006, -0.999580136,  0.0000368439, -0.0307300513]
+       - [-0.0094993062, -0.0003122155, -0.999954834,  -0.386689354 ]
+       - [ 0.999534999,   0.0289731321, -0.0095043721, -0.671425124 ]
+       - [ 0.0,           0.0,           0.0,           1.0      ]
+     T_l2v:
+       - [ 0.99939639, 0.02547917, 0.023615, 1.1 ]
+       - [ -0.02530848, 0.99965156, -0.00749882, 0.03773583 ]
+       - [ -0.02379784, 0.00689664, 0.999693, 1.95320223 ]
+       - [ 0.0,          0.0,          0.0,          1.0 ]
+
+   front_right:
+     K:
+       - [1176.25545,    0.0,          966.432645]
+       - [0.0,          1175.14569,    608.580326]
+       - [0.0,          0.0,          1.0       ]
+     D: [-0.270136325, 0.164393255, -0.00160720782, -0.0000741246708, -0.0619939758]
+     T_l2c:
+       - [-0.71836368, -0.69527204, -0.02346088,  0.05718003]
+       - [-0.09720448,  0.13371206, -0.98624154, -0.15983010]
+       - [ 0.68884317, -0.70619960, -0.16363744, -1.04767285]
+       - [ 0.0,          0.0,          0.0,          1.0      ]
+     T_l2v:
+       - [0.99939639, 0.02547917, 0.023615, 1.1]
+       - [-0.02530848, 0.99965156, -0.00749882, 0.03773583]
+       - [-0.02379784, 0.00689664, 0.999693, 1.95320223]
+       - [0.0,          0.0,          0.0,          1.0 ]
+

GEMstack/knowledge/calibration/gem_e4.yaml

@@ -1,7 +1,7 @@
-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
-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?
+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"
 front_camera: !include "gem_e4_oak.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/detection/bevfusion_instructions.md

@@ -0,0 +1,146 @@
+# BEVFusion Set Up Instructions
+These instructions were tested on T4 g4dn.xlarge AWS instances with Arara Ubuntu 20.04 DCV images.
+
+## Set Up Instructions for Cuda 11.3
+### Set Up your Nvida Driver
+```
+sudo apt-get update
+sudo apt-get install -y ubuntu-drivers-common
+ubuntu-drivers devices
+sudo apt-get install -y nvidia-driver-565
+sudo reboot
+```
+
+### Check to make sure that your nvidia driver was set up correctly:
+```
+nvidia-smi
+```
+
+### Install Cuda 11.3
+```
+wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin
+sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600
+wget https://developer.download.nvidia.com/compute/cuda/11.3.0/local_installers/cuda-repo-ubuntu2004-11-3-local_11.3.0-465.19.01-1_amd64.deb
+sudo dpkg -i cuda-repo-ubuntu2004-11-3-local_11.3.0-465.19.01-1_amd64.deb # This never copy pastes right. Just manually type
+sudo apt-key add /var/cuda-repo-ubuntu2004-11-3-local/7fa2af80.pub
+sudo apt-get update
+sudo apt-get -y install cuda-11-3
+```
+
+### Manually modify your bashrc file to include Cuda 11.3
+```
+sudo nano ~/.bashrc 
+```
+
+Add the next 2 lines to the bottom of the file:
+```
+export PATH=/usr/local/cuda-11.3/bin:$PATH
+export LD_LIBRARY_PATH=/usr/local/cuda-11.3/lib64:$LD_LIBRARY_PATH
+```
+
+Ensure you source your bashrc file:
+```
+source ~/.bashrc
+nvidia-smi
+```
+
+### Set Up Miniconda
+```
+wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
+bash ~/Miniconda3-latest-Linux-x86_64.sh
+source ~/.bashrc
+```
+
+### Set Up mmdetection3d:
+```
+conda create --name openmmlab python=3.8 -y
+conda activate openmmlab
+conda install pytorch=1.10.2 torchvision=0.11.3 cudatoolkit=11.3 -c pytorch
+pip install -U openmim
+mim install mmengine
+mim install 'mmcv>=2.0.0rc4, <2.2.0'
+mim install 'mmdet>=3.0.0,<3.3.0'
+pip install cumm-cu113
+pip install spconv-cu113
+git clone https://github.com/open-mmlab/mmdetection3d.git -b dev-1.x
+cd mmdetection3d
+pip install -v -e .
+python projects/BEVFusion/setup.py develop
+```
+
+### Run this afterwards to verify BEVFusion has been set up correctly:
+```
+python projects/BEVFusion/demo/multi_modality_demo.py demo/data/nuscenes/n015-2018-07-24-11-22-45+0800__LIDAR_TOP__1532402927647951.pcd.bin demo/data/nuscenes/ demo/data/nuscenes/n015-2018-07-24-11-22-45+0800.pkl projects/BEVFusion/configs/bevfusion_lidar-cam_voxel0075_second_secfpn_8xb4-cyclic-20e_nus-3d.py ~/Downloads/bevfusion_lidar-cam_voxel0075_second_secfpn_8xb4-cyclic-20e_nus-3d-5239b1af.pth --cam-type all --score-thr 0.2 --show
+```
+
+## Set Up Instructions for Cuda 11.1
+### Set Up your Nvida Driver
+```
+sudo apt-get update
+sudo apt-get install -y ubuntu-drivers-common
+ubuntu-drivers devices
+sudo apt-get install -y nvidia-driver-565
+sudo reboot
+```
+
+### Check to make sure that your nvidia driver was set up correctly:
+```
+nvidia-smi
+```
+
+### Install Cuda 11.3
+```
+wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin
+sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600
+wget https://developer.download.nvidia.com/compute/cuda/11.1.0/local_installers/cuda-repo-ubuntu2004-11-1-local_11.1.0-455.23.05-1_amd64.deb
+sudo dpkg -i cuda-repo-ubuntu2004-11-1-local_11.1.0-455.23.05-1_amd64.deb
+sudo apt-key add /var/cuda-repo-ubuntu2004-11-1-local/7fa2af80.pub
+sudo apt-get update
+sudo apt-get -y install cuda-11-1
+```
+
+### Manually modify your bashrc file to include Cuda 11.3
+```
+sudo nano ~/.bashrc 
+```
+
+Add the next 2 lines to the bottom of the file:
+```
+export PATH=/usr/local/cuda-11.1/bin:$PATH
+export LD_LIBRARY_PATH=/usr/local/cuda-11.1/lib64:$LD_LIBRARY_PATH
+```
+
+Ensure you source your bashrc file:
+```
+source ~/.bashrc
+nvidia-smi
+```
+
+### Set Up Miniconda
+```
+wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
+bash ~/Miniconda3-latest-Linux-x86_64.sh
+source ~/.bashrc
+```
+
+### Set Up mmdetection3d:
+```
+conda create --name openmmlab python=3.8 -y
+conda activate openmmlab
+pip install torch==1.10.0+cu111 torchvision==0.11.1+cu111 -f https://download.pytorch.org/whl/torch_stable.html
+pip install -U openmim
+mim install mmengine
+mim install 'mmcv>=2.0.0rc4, <2.2.0'
+mim install 'mmdet>=3.0.0,<3.3.0'
+pip install cumm-cu111
+pip install spconv-cu111
+git clone https://github.com/open-mmlab/mmdetection3d.git -b dev-1.x
+cd mmdetection3d
+pip install -v -e .
+python projects/BEVFusion/setup.py develop
+```
+
+### Run this afterwards to verify BEVFusion has been set up correctly:
+```
+python projects/BEVFusion/demo/multi_modality_demo.py demo/data/nuscenes/n015-2018-07-24-11-22-45+0800__LIDAR_TOP__1532402927647951.pcd.bin demo/data/nuscenes/ demo/data/nuscenes/n015-2018-07-24-11-22-45+0800.pkl projects/BEVFusion/configs/bevfusion_lidar-cam_voxel0075_second_secfpn_8xb4-cyclic-20e_nus-3d.py ~/Downloads/bevfusion_lidar-cam_voxel0075_second_secfpn_8xb4-cyclic-20e_nus-3d-5239b1af.pth --cam-type all --score-thr 0.2 --show
+```

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)