krishauser · alo-20 · Feb 17, 2025 · Feb 18, 2025 · Feb 18, 2025 · Feb 19, 2025
@@ -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,8 @@ cython_debug/
 
 .vscode/
 setup/zed_sdk.run
-cuda/
+
+cuda/
+#Ignore ROS bags
+*.bag
+
@@ -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"
@@ -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]
+                ]
@@ -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)
@@ -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]]
@@ -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: 
@@ -1,34 +1,54 @@
 # ********* Main settings entry point for behavior stack ***********
-
-# Configure settings for the vehicle / vehicle model
-vehicle:  !include ../vehicle/gem_e4.yaml
-
-#arguments for algorithm components here
-model_predictive_controller:
-    dt: 0.1
-    lookahead: 20
-control:
-    recovery:
-        brake_amount : 0.5
-        brake_speed : 2.0
-    pure_pursuit:
-        lookahead: 2.0
-        lookahead_scale: 3.0
-        crosstrack_gain: 1.0
-        desired_speed: trajectory
-    longitudinal_control:
-        pid_p: 1.0
-        pid_i: 0.1
-        pid_d: 0.0
-
-#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
-        #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
+
+ # Configure settings for the vehicle / vehicle model
+ vehicle:  !include ../vehicle/gem_e4.yaml
+
+ #arguments for algorithm components here
+ model_predictive_controller:
+     dt: 0.1
+     lookahead: 20
+ control:
+     recovery:
+         brake_amount : 0.5
+         brake_speed : 2.0
+     pure_pursuit:
+         lookahead: 2.0
+         lookahead_scale: 2.0
+         crosstrack_gain: 0.3
+         desired_speed: trajectory #racing
+     launch_control:
+         enable: 1
+         stage_duration: 0.5
+     # Stanley controller parameters (fine tune this)
+     stanley:
+         control_gain: 0.1
+         softening_gain: 0.2
+         desired_speed: racing        # Speed reference source: can be "trajectory", "path", "racing" or a constant value
+
+     longitudinal_control:
+         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.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
@@ -0,0 +1,100 @@
+10.0,0.0
+9.980267284282716,0.6279051952931337
+9.921147013144777,1.2533323356430426
+9.822872507286887,1.8738131458572465
+9.685831611286313,2.4868988716485485
+9.510565162951536,3.0901699437494745
+9.297764858882513,3.68124552684678
+9.048270524660193,4.257792915650727
+8.763066800438635,4.817536741017153
+8.443279255020151,5.3582679497899655
+8.090169943749476,5.87785252292473
+7.705132427757894,6.3742398974868975
+7.289686274214117,6.845471059286887
+6.845471059286887,7.289686274214117
+6.374239897486897,7.705132427757894
+5.87785252292473,8.090169943749476
+5.3582679497899655,8.443279255020151
+4.817536741017151,8.763066800438637
+4.257792915650727,9.048270524660195
+3.681245526846779,9.297764858882514
+3.090169943749475,9.510565162951536
+2.486898871648548,9.685831611286313
+1.8738131458572451,9.822872507286887
+1.2533323356430426,9.921147013144777
+0.6279051952931329,9.980267284282716
+-1.6081226496766366e-15,9.999999999999998
+-0.627905195293134,9.980267284282716
+-1.2533323356430437,9.921147013144777
+-1.8738131458572485,9.822872507286887
+-2.486898871648549,9.685831611286313
+-3.0901699437494763,9.510565162951536
+-3.68124552684678,9.297764858882514
+-4.257792915650728,9.048270524660195
+-4.817536741017154,8.763066800438635
+-5.358267949789967,8.44327925502015
+-5.877852522924729,8.090169943749476
+-6.374239897486897,7.705132427757894
+-6.845471059286887,7.289686274214116
+-7.289686274214117,6.845471059286886
+-7.705132427757895,6.374239897486894
+-8.090169943749475,5.877852522924732
+-8.443279255020151,5.3582679497899655
+-8.763066800438637,4.817536741017151
+-9.048270524660195,4.257792915650725
+-9.297764858882514,3.6812455268467774
+-9.510565162951538,3.090169943749471
+-9.685831611286313,2.4868988716485485
+-9.822872507286887,1.8738131458572458
+-9.921147013144777,1.2533323356430408
+-9.980267284282716,0.6279051952931314
+-10.0,-3.216245299353273e-15
+-9.980267284282716,-0.6279051952931334
+-9.921147013144777,-1.253332335643043
+-9.822872507286887,-1.873813145857248
+-9.685831611286313,-2.4868988716485503
+-9.510565162951535,-3.090169943749478
+-9.297764858882513,-3.6812455268467836
+-9.048270524660193,-4.257792915650727
+-8.763066800438635,-4.817536741017154
+-8.44327925502015,-5.358267949789967
+-8.090169943749473,-5.877852522924733
+-7.705132427757891,-6.3742398974869
+-7.289686274214117,-6.845471059286887
+-6.845471059286886,-7.289686274214117
+-6.374239897486895,-7.705132427757895
+-5.877852522924733,-8.090169943749475
+-5.358267949789962,-8.443279255020153
+-4.817536741017153,-8.763066800438635
+-4.257792915650722,-9.048270524660197
+-3.6812455268467783,-9.297764858882514
+-3.0901699437494763,-9.510565162951535
+-2.4868988716485445,-9.685831611286313
+-1.8738131458572465,-9.822872507286887
+-1.2533323356430373,-9.921147013144777
+-0.6279051952931319,-9.980267284282716
+-1.8369701987210296e-15,-10.0
+0.6279051952931372,-9.980267284282716
+1.2533323356430424,-9.921147013144777
+1.8738131458572516,-9.822872507286887
+2.48689887164855,-9.685831611286313
+3.0901699437494727,-9.510565162951538
+3.681245526846783,-9.297764858882513
+4.257792915650727,-9.048270524660195
+4.817536741017157,-8.763066800438633
+5.358267949789966,-8.44327925502015
+5.877852522924735,-8.090169943749471
+6.374239897486898,-7.705132427757891
+6.845471059286886,-7.289686274214117
+7.289686274214119,-6.845471059286883
+7.705132427757894,-6.374239897486895
+8.09016994374948,-5.877852522924725
+8.443279255020153,-5.358267949789962
+8.763066800438635,-4.817536741017153
+9.048270524660197,-4.257792915650722
+9.297764858882513,-3.6812455268467787
+9.510565162951538,-3.0901699437494683
+9.685831611286313,-2.4868988716485454
+9.822872507286887,-1.8738131458572471
+9.921147013144777,-1.253332335643038
+9.980267284282716,-0.6279051952931326
@@ -0,0 +1,100 @@
+20.0,0.0
+19.960534568565432,1.2558103905862674
+19.842294026289558,2.506664671286085
+19.645745014573773,3.7476262917144925
+19.37166322257262,4.973797743297096
+19.02113032590307,6.180339887498948
+18.595529717765025,7.362491053693559
+18.09654104932039,8.515585831301454
+17.526133600877273,9.635073482034306
+16.886558510040302,10.716535899579933
+16.18033988749895,11.755705045849464
+15.410264855515782,12.748479794973795
+14.579372548428232,13.690942118573775
+13.690942118573773,14.579372548428232
+12.748479794973793,15.410264855515784
+11.75570504584946,16.18033988749895
+10.716535899579931,16.886558510040302
+9.635073482034302,17.526133600877273
+8.515585831301454,18.096541049320393
+7.3624910536935575,18.59552971776503
+6.180339887498949,19.02113032590307
+4.973797743297094,19.37166322257262
+3.7476262917144902,19.645745014573773
+2.506664671286085,19.842294026289558
+1.255810390586266,19.960534568565432
+-3.2162452993532727e-15,20.0
+-1.255810390586268,19.960534568565432
+-2.5066646712860874,19.842294026289554
+-3.7476262917144965,19.645745014573773
+-4.973797743297097,19.37166322257262
+-6.180339887498951,19.02113032590307
+-7.362491053693559,18.595529717765025
+-8.515585831301454,18.09654104932039
+-9.635073482034308,17.52613360087727
+-10.716535899579938,16.8865585100403
+-11.75570504584946,16.18033988749895
+-12.748479794973795,15.410264855515784
+-13.690942118573775,14.579372548428228
+-14.579372548428234,13.69094211857377
+-15.410264855515788,12.74847979497379
+-16.180339887498945,11.755705045849465
+-16.886558510040302,10.716535899579933
+-17.526133600877273,9.635073482034304
+-18.096541049320393,8.51558583130145
+-18.59552971776503,7.362491053693555
+-19.021130325903073,6.180339887498942
+-19.37166322257262,4.973797743297096
+-19.645745014573773,3.7476262917144916
+-19.842294026289558,2.5066646712860816
+-19.960534568565432,1.2558103905862628
+-20.0,-6.432490598706546e-15
+-19.960534568565432,-1.255810390586267
+-19.842294026289558,-2.5066646712860856
+-19.645745014573773,-3.7476262917144956
+-19.37166322257262,-4.973797743297101
+-19.02113032590307,-6.180339887498954
+-18.595529717765025,-7.362491053693566
+-18.09654104932039,-8.515585831301454
+-17.52613360087727,-9.635073482034308
+-16.8865585100403,-10.716535899579934
+-16.180339887498945,-11.755705045849467
+-15.41026485551578,-12.7484797949738
+-14.579372548428232,-13.690942118573775
+-13.690942118573773,-14.579372548428232
+-12.74847979497379,-15.410264855515788
+-11.755705045849465,-16.180339887498945
+-10.716535899579927,-16.886558510040306
+-9.635073482034306,-17.526133600877273
+-8.515585831301443,-18.096541049320397
+-7.362491053693557,-18.59552971776503
+-6.180339887498951,-19.02113032590307
+-4.973797743297089,-19.371663222572625
+-3.7476262917144925,-19.645745014573773
+-2.5066646712860745,-19.842294026289558
+-1.255810390586264,-19.960534568565432
+-3.673940397442059e-15,-20.0
+1.2558103905862743,-19.960534568565432
+2.5066646712860847,-19.842294026289558
+3.7476262917145027,-19.645745014573773
+4.973797743297099,-19.37166322257262
+6.180339887498945,-19.021130325903073
+7.3624910536935655,-18.595529717765025
+8.515585831301452,-18.096541049320393
+9.635073482034315,-17.526133600877266
+10.716535899579934,-16.8865585100403
+11.755705045849474,-16.18033988749894
+12.7484797949738,-15.41026485551578
+13.690942118573773,-14.579372548428232
+14.579372548428237,-13.690942118573766
+15.410264855515788,-12.748479794973793
+16.180339887498956,-11.755705045849451
+16.886558510040306,-10.716535899579927
+17.526133600877273,-9.635073482034306
+18.096541049320393,-8.515585831301443
+18.59552971776503,-7.3624910536935575
+19.021130325903076,-6.180339887498936
+19.371663222572625,-4.97379774329709
+19.645745014573773,-3.747626291714494
+19.842294026289558,-2.506664671286076
+19.960534568565432,-1.2558103905862652