krishauser · harishkumarbalaji · Apr 23, 2025 · Jan 27, 2025 · Jan 28, 2025 · Jan 29, 2025
diff --git a/.gitignore b/.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
diff --git a/GEMstack/knowledge/calibration/gem_e2_gazebo.yaml b/GEMstack/knowledge/calibration/gem_e2_gazebo.yaml
@@ -0,0 +1,59 @@
+calibration_date: "2023-08-31"  # Date of calibration YYYY-MM-DD
+reference: rear_axle_center  # rear axle center
+rear_axle_height: 0.273      # height of rear axle center above flat ground
+gnss_location: [1.1,0.0,0.3] # 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
+front_radar_location: [1.45,0,0.07] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/platform_launch/launch/white_e2/platform.launch
+top_lidar: !include "gem_e2_velodyne.yaml"
+front_camera: !include "gem_e2_zed.yaml"
+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
diff --git a/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml b/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml
@@ -0,0 +1,64 @@
+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"
+gnss_reference_point: [40.114001, -88.228837]
+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
diff --git a/GEMstack/knowledge/defaults/__init__.py b/GEMstack/knowledge/defaults/__init__.py
@@ -1,7 +1,36 @@
 from ...utils.config import load_config_recursive
-import os
-SETTINGS_FILE = os.path.join(os.path.split(__file__)[0],'current.yaml')
+import os,sys
+
+
+# Identify the vehicle used and load settings else default to vehicle config at current.yaml'
+# default currently uses GEM e4
+vehicle = "default"
+
+for arg in sys.argv:
+    if arg.startswith('--'):
+        k,v = arg.split('=',1)
+        k = k[2:]
+        v = v.strip('"')
+        if k == "vehicle":
+            vehicle = v
+            sys.argv.remove(arg)
+            break
+
+if(vehicle == 'e2'):
+    vehicle_config = 'e2.yaml'
+elif(vehicle == 'e4'):
+    vehicle_config = 'current.yaml'
+elif(vehicle == 'e2_gazebo'):
+    vehicle_config = 'e2_gazebo.yaml'
+elif(vehicle == 'e4_gazebo'):
+    vehicle_config = 'e4_gazebo.yaml'
+else:
+    print("Unknown vehicle argument passed")
+    vehicle = "default"
+    vehicle_config = 'current.yaml'
+
+SETTINGS_FILE = os.path.join(os.path.split(__file__)[0],vehicle_config)
 print("**************************************************************")
-print("Loading default settings from",SETTINGS_FILE)
+print(f"Loading {vehicle} settings from",SETTINGS_FILE)
 print("**************************************************************")
 SETTINGS = load_config_recursive(SETTINGS_FILE)
diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml
@@ -1,8 +1,7 @@
 # ********* Main settings entry point for behavior stack ***********
 
 # Configure settings for the vehicle / vehicle model
-# vehicle:  !include ../vehicle/gem_e4.yaml
-vehicle:  !include ../vehicle/gem_e2_gazebo.yaml
+vehicle:  !include ../vehicle/gem_e4.yaml
 
 #arguments for algorithm components here
 model_predictive_controller:
@@ -13,8 +12,8 @@ control:
         brake_amount : 0.5
         brake_speed : 2.0
     pure_pursuit:
-        lookahead: 2.0
-        lookahead_scale: 3.0
+        lookahead: 4.0
+        lookahead_scale: 1.0
         crosstrack_gain: 1.0
         desired_speed: trajectory
     longitudinal_control:

diff --git a/GEMstack/knowledge/defaults/e2.yaml b/GEMstack/knowledge/defaults/e2.yaml
@@ -0,0 +1,34 @@
+# ********* Main settings entry point for behavior stack ***********
+
+# Configure settings for the vehicle / vehicle model
+vehicle:  !include ../vehicle/gem_e2.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
diff --git a/GEMstack/knowledge/defaults/e2_gazebo.yaml b/GEMstack/knowledge/defaults/e2_gazebo.yaml
@@ -0,0 +1,34 @@
+# ********* Main settings entry point for behavior stack ***********
+
+# Configure settings for the vehicle / vehicle model
+vehicle:  !include ../vehicle/gem_e2_gazebo.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: 4.0
+        lookahead_scale: 3.0
+        crosstrack_gain: 1.0
+        desired_speed: trajectory
+    longitudinal_control:
+        pid_p: 0.1
+        pid_i: 0.5
+        pid_d: 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
+        #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
diff --git a/GEMstack/knowledge/defaults/e4_gazebo.yaml b/GEMstack/knowledge/defaults/e4_gazebo.yaml
@@ -0,0 +1,34 @@
+# ********* Main settings entry point for behavior stack ***********
+
+# Configure settings for the vehicle / vehicle model
+vehicle:  !include ../vehicle/gem_e4_gazebo.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.5
+        lookahead_scale: 3.0
+        crosstrack_gain: 0.5
+        desired_speed: trajectory
+    longitudinal_control:
+        pid_p: 1.5
+        pid_i: 0.2
+        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
diff --git a/GEMstack/knowledge/vehicle/gem_e2_gazebo.yaml b/GEMstack/knowledge/vehicle/gem_e2_gazebo.yaml
@@ -9,5 +9,5 @@ geometry: !include gem_e2_geometry.yaml
 dynamics: !include gem_e2_dynamics.yaml
 limits: !include gem_e2_slow_limits.yaml
 control_defaults: !include gem_e2_control_defaults.yaml
-calibration: !include ../calibration/gem_e2.yaml
+calibration: !include ../calibration/gem_e2_gazebo.yaml
 sensors: !include gem_e2_sensor_environment_gazebo.yaml
diff --git a/GEMstack/knowledge/vehicle/gem_e2_sensor_environment_gazebo.yaml b/GEMstack/knowledge/vehicle/gem_e2_sensor_environment_gazebo.yaml
@@ -1,6 +1,7 @@
 ros_topics:
   top_lidar: /velodyne_points
   front_camera: /front_single_camera/image_raw
-  front_depth: /zed2/zed_node/depth/depth_registered
-  gnss: /gps/fix
+  front_depth: /notopic
+  gps: /gps/fix
+  imu: /imu
 
diff --git a/GEMstack/knowledge/vehicle/gem_e4_gazebo.yaml b/GEMstack/knowledge/vehicle/gem_e4_gazebo.yaml
@@ -0,0 +1,13 @@
+name: GEM
+version: e4
+max_gear : 1
+num_wiper_settings : 1
+enable_through_joystick : true   #turn this to false to have GEMstack enable control
+max_command_rate : 10.0          #for hardware, max rate of commands to send to vehicle over Pacmod
+#using !include directives helps maintain reuse of common settings
+geometry: !include gem_e4_geometry.yaml
+dynamics: !include gem_e4_dynamics.yaml
+limits: !include gem_e2_slow_limits.yaml
+control_defaults: !include gem_e2_control_defaults.yaml
+calibration: !include ../calibration/gem_e4_gazebo.yaml
+sensors: !include gem_e4_sensor_environment_gazebo.yaml
diff --git a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment_gazebo.yaml b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment_gazebo.yaml
@@ -0,0 +1,7 @@
+ros_topics:
+  top_lidar: /velodyne_points
+  front_camera: /front_single_camera/image_raw
+  front_depth: /notopic
+  gps: /gps/fix
+  imu: /imu
+