Merge branch 's2025_EstCal_Calibration_PR' of https://github.com/krishauser/GEMstack into s2025_EstCal_Calibration_PR

Author: michalj1

.github/workflows/python-app.yml

@@ -30,7 +30,7 @@ jobs:
     - 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
+        flake8 ./GEMstack --count --select=E9,F63,F7,F82 --ignore=F824 --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

.gitignore

@@ -22,6 +22,7 @@ downloads/
 eggs/
 .eggs/
 lib/
+!frontend/*
 lib64/
 parts/
 sdist/
@@ -32,6 +33,7 @@ share/python-wheels/
 .installed.cfg
 *.egg
 MANIFEST
+.idea/
 
 # PyInstaller
 #  Usually these files are written by a python script from a template
@@ -134,6 +136,7 @@ venv/
 ENV/
 env.bak/
 venv.bak/
+*.DS_Store
 
 # Spyder project settings
 .spyderproject
@@ -164,4 +167,22 @@ cython_debug/
 #  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
 #  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/
+
+.idea/
+
+# ZED run files
+**/*.run
+.vscode/
+setup/zed_sdk.run
+
+#Ignore ROS bags
+*.bag
+
+cuda/
+homework/yolov8n.pt
+homework/yolo11n.pt
+yolov8n.pt
+yolo11n.pt
+
+# Computation  Graph   of Launch File Outputs
+launch_visualization/graph

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_ouster.yaml

@@ -1,3 +1,5 @@
 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.10, 0.03773583, 1.95320223] # meters, calibrated by Hang's watchful eye
+rotation: [[0.99939639, 0.02547917, 0.023615], 
+           [-0.02530848, 0.99965156, -0.00749882],
+           [-0.02379784, 0.00689664, 0.999693]]  #rotation matrix mapping lidar frame to vehicle frame

GEMstack/knowledge/defaults/ReedsShepp_param.yaml

@@ -0,0 +1,19 @@
+# vehicle info 
+vehicle:
+    vehicle_dim: [1.7, 3.2]  # in meter
+    vehicle_turning_radius: 3.657
+# algorithm parameters
+reeds_shepp_parking:
+    shift_from_center_to_rear_axis: 1.25 # in meter
+    search_step_size: 0.1 # in meter
+    closest: False # If True, the closest parking spot will be selected, otherwise the farthest one will be selected
+    parking_lot_axis_shift_margin: 2.44 # in meter
+    search_bound_threshold: 0.5 
+    clearance_step: 0.5
+    clearance: 0
+    add_static_vertical_curb_as_obstacle: True
+    add_static_horizontal_curb_as_obstacle: True
+    static_horizontal_curb_size: [2.44, 0.5]
+    static_vertical_curb_size: [2.44, 24.9]
+    compact_parking_spot_size: [2.44, 4.88] # US Compact Space for parking (2.44, 4.88)
+

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -21,6 +21,9 @@ components:
   - lane_detection:
       inputs: [vehicle, roadgraph]
       outputs: vehicle_lane
+  - parking_detection:
+      inputs: obstacles
+      outputs: [goal, obstacles]
   - sign_detection: 
       inputs: [vehicle, roadgraph]
       outputs: roadgraph.signs
@@ -40,15 +43,33 @@ components:
       inputs: all
   - mission_execution:
       outputs: mission
+  - mission_planning:
+      inputs: all
+      outputs: mission
   - route_planning:
-      inputs: [vehicle, roadgraph, mission]
+      inputs: all
       outputs: route
-  - driving_logic:
+  - route_planning_component:
+      inputs: all
+      outputs: [ route, mission ]
+  - save_lidar_data:
       inputs: all
+      outputs:
+  - driving_logic:
+      inputs: 
       outputs: intent
+  - _mission_planner: # one way
+      inputs: all
+      outputs: mission_plan
+  - _route_planner: # 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/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

GEMstack/knowledge/defaults/rrt_param.yaml

@@ -0,0 +1,19 @@
+# arguments for BiRRT* algorithm
+# map parameters
+map:
+    lower_x: -15 
+    upper_x: 40 
+    lower_y: -10
+    upper_y: 30
+    grid_resolution: 10 # grids per meter for occupency grid
+    lane_boundary_radius: 0.2 # radius in meter for determining the size of the lane boundary in occupency grid
+    obstacle_radius: 0.2 # radius in meter for determining the size of the obstacle in occupency grid
+# vehicle info 
+vehicle:
+    heading_limit: 0.5235987756  # vehicle heading difference limit per rrt step_size (pi/6 in radians)
+    half_width: 0.8 # vehicle half width in meter
+# algorithm parameters
+rrt:
+    time_limit: 10 # in sec
+    step_size: 0.5 # length in meter for local planner to explore
+    search_r: 1.4 # radius in meter for rewiring