Merge s2025 latest into s2025_perception_merge_Tianyu

.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,8 +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
-cuda/
+
+#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/calib_util.py

@@ -0,0 +1,106 @@
+#%%
+import yaml
+from yaml import SafeDumper
+import numpy as np
+import cv2
+def represent_flow_style_list(dumper, data):
+    return dumper.represent_sequence(yaml.resolver.BaseResolver.DEFAULT_SEQUENCE_TAG, data, flow_style=True)
+SafeDumper.add_representer(list, represent_flow_style_list)
+#%%
+class FlowListDumper(yaml.Dumper):
+    def represent_list(self, data):
+        return self.represent_sequence('tag:yaml.org,2002:seq', data, flow_style=True)
+
+def load_ex(path,mode,ref='rear_axle_center'):
+    with open(path) as stream:
+        y = yaml.safe_load(stream)
+    assert y['reference'] == ref
+    if mode == 'matrix':
+        ret = np.eye(4)
+        ret[0:3,0:3] = y['rotation']
+        ret[:-1,3] = y['position']
+        return ret
+    elif mode == 'tuple':
+        return np.array(y['rotation']),np.array(y['position'])
+
+
+def save_ex(path,rotation=None,translation=None,matrix=None,ref='rear_axle_center'):
+    if matrix is not None:
+        rot = matrix[0:3,0:3]
+        trans = matrix[0:3,3]
+        save_ex(path,rot,trans,ref=ref)
+        return
+    ret = {}
+    ret['reference'] = ref
+    ret['rotation'] = rotation
+    ret['position'] = translation
+    for i in ret:
+        if type(ret[i]) == np.ndarray:
+            ret[i] = ret[i].tolist()
+    print(yaml.dump(ret,Dumper=SafeDumper,default_flow_style=False))
+    with open(path,'w') as stream:
+        yaml.dump(ret,stream,Dumper=SafeDumper,default_flow_style=False)
+
+def load_in(path,mode='matrix',return_distort=False):
+    with open(path) as stream:
+        y = yaml.safe_load(stream)
+    if 'skew' not in y: y['skew'] = 0
+    if 'distort' not in y: y['distort'] = [0,0,0,0,0]
+    if mode == 'matrix':
+        ret = np.zeros((3,3))
+        ret[0,0],ret[1,1] = y['focal']
+        ret[2,2] = 1.
+        ret[0:2,2] = y['center']
+        ret[0,1] = y['skew']
+        if return_distort:
+            return ret,np.array(y['distort'])
+        else:
+            return ret
+    elif mode == 'tuple':
+        return {'focal':np.array(y['focal']),
+                'center':np.array(y['center']),
+                'skew':np.array(y['skew']),
+                'distort':np.array(y['distort'])}
+
+from collections.abc import Iterable
+def save_in(path,focal=None,center=None,skew=0,distort=[0.0]*5,matrix=None):
+    if matrix is not None:
+        focal = matrix.diagonal()[0:2]
+        skew = matrix[0,1]
+        center = matrix[0:2,2]
+        save_in(path,focal,center,skew,distort)
+        return
+    ret = {}
+    ret['focal'] = focal
+    ret['center'] = center
+    ret['skew'] = skew 
+    assert len(distort) in [4,5]
+    ret['distort'] = distort
+    if len(ret['distort']) == 4:
+        ret['distort'] = list(ret['distort'])+[0.0]
+    for i in ret:
+        if type(ret[i]) == np.ndarray:
+            ret[i] = ret[i].tolist()
+        if isinstance(ret[i],Iterable):
+            ret[i] = [*map(float,ret[i])]
+    print(yaml.dump(ret,Dumper=SafeDumper,default_flow_style=False))
+    with open(path,'w') as stream:
+        yaml.dump(ret,stream,Dumper=SafeDumper,default_flow_style=False)
+
+def undistort_image(image, camera_matrix, distortion_coefficients):
+    h,  w = image.shape[:2]
+    newK, roi = cv2.getOptimalNewCameraMatrix(camera_matrix, distortion_coefficients, (w,h), 1, (w,h))
+    image = cv2.undistort(image, camera_matrix, distortion_coefficients, None, newK)
+    return image, newK
+
+
+#%%
+if __name__ == "__main__":
+    #%%
+    rot, trans = load_ex('/mnt/GEMstack/GEMstack/knowledge/calibration/gem_e4_ouster.yaml',mode='tuple')
+    save_ex('/tmp/test.yaml',rot,trans)
+    #%%
+    focal = [1,2,3]
+    center = [400,500]
+    save_in('/tmp/test.yaml',focal,center)
+    load_in('/tmp/test.yaml',mode='tuple')

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,21 @@
 calibration_date: "2024-03-05"  # 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?
+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"
+front_camera:
+  extrinsics: !include "gem_e4_oak.yaml"
+  intrinsics: !include "gem_e4_oak_in.yaml"
+front_right_camera:
+  extrinsics: !include "gem_e4_fr.yaml"
+  intrinsics: !include "gem_e4_fr_in.yaml"
+front_left_camera:
+  extrinsics: !include "gem_e4_fl.yaml"
+  intrinsics: !include "gem_e4_fl_in.yaml"
+rear_right_camera:
+  extrinsics: !include "gem_e4_rr.yaml"
+  intrinsics: !include "gem_e4_rr_in.yaml"
+rear_left_camera:
+  extrinsics: !include "gem_e4_rl.yaml"
+  intrinsics: !include "gem_e4_rl_in.yaml"

GEMstack/knowledge/calibration/gem_e4_fl.yaml

@@ -0,0 +1,5 @@
+position: [2.008491967178938, 0.9574436688609637, 1.7222845229507735]
+reference: rear_axle_center
+rotation: [[0.7229102844527417, -0.13938889438297952, 0.6767358840457229], [-0.6904150547378912,
+    -0.18396833469067211, 0.6996304053015531], [0.026977264941612008, -0.9729986577348562,
+    -0.22922879230680995]]

GEMstack/knowledge/calibration/gem_e4_fl_in.yaml

@@ -0,0 +1,5 @@
+center: [971.5122150421694, 601.7847095069886]
+distort: [-0.2625420437513607, 0.1425651774165483, -0.0004946279626072071, -0.00033457504102070386,
+  -0.042732740327368145]
+focal: [1183.2337731693713, 1182.3831532373445]
+skew: 0

GEMstack/knowledge/calibration/gem_e4_fr.yaml

@@ -0,0 +1,5 @@
+position: [1.8861563355156226, -0.7733611068168774, 1.6793040225335112]
+reference: rear_axle_center
+rotation: [[-0.7168464770690616, -0.10046018208578958, 0.6899557088168523], [-0.6970911725372957,
+    0.12308618950445319, -0.7063382243117325], [-0.01396515249660048, -0.9872981017750231,
+    -0.15826380744561577]]

GEMstack/knowledge/calibration/gem_e4_fr_in.yaml

@@ -0,0 +1,5 @@
+center: [966.4326452411585, 608.5803255934914]
+distort: [-0.2701363254469883, 0.16439325523243875, -0.001607207824773341, -7.412467081891699e-05,
+  -0.06199397580030171]
+focal: [1176.2554468073797, 1175.1456876174707]
+skew: 0

GEMstack/knowledge/calibration/gem_e4_oak.yaml

@@ -1,3 +1,5 @@
-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
+position: [1.8680678362969751, 0.03483728869549903, 1.6545932338230158]
+reference: rear_axle_center
+rotation: [[0.020064651878799838, -0.013111205776054045, 0.99971271174677], [-0.9997929081548379,
+    0.0031358785499412617, 0.020107388418472497], [-0.003398609756043868, -0.9999091271454714,
+    -0.013045570240818732]]

GEMstack/knowledge/calibration/gem_e4_oak_in.yaml

@@ -0,0 +1,2 @@
+center: [573.37109375, 363.700927734375]
+focal: [684.8333129882812, 684.6096801757812]

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/calibration/gem_e4_rl.yaml

@@ -0,0 +1,5 @@
+position: [0.0898392124024201, 0.71876803481624, 1.71024199833245]
+reference: rear_axle_center
+rotation: [[0.6847850928670124, 0.19803293816635642, -0.7013218462220591], [0.728026894383745,
+    -0.14318896257364072, 0.6704280439025839], [0.03234528777238433, -0.9696802959730001,
+    -0.2422269719924619]]

GEMstack/knowledge/calibration/gem_e4_rl_in.yaml

@@ -0,0 +1,5 @@
+center: [953.302889408274, 608.1966398872765]
+distort: [-0.2522996862206216, 0.12482113115174773, -0.0005993692936397102, -0.00017949453391219192,
+  -0.03499498178003368]
+focal: [1181.6177321982138, 1180.0783789769903]
+skew: 0

GEMstack/knowledge/calibration/gem_e4_rr.yaml

@@ -0,0 +1,5 @@
+position: [0.11419591502518789, -0.6896311735924415, 1.711181163333824]
+reference: rear_axle_center
+rotation: [[-0.7359657309159472, 0.15986191414426415, -0.6578743127098735], [0.6768157805459531,
+    0.14993386619459964, -0.7207220233709469], [-0.016578363047300385, -0.9756864271752846,
+    -0.21854325362408236]]

GEMstack/knowledge/calibration/gem_e4_rr_in.yaml

@@ -0,0 +1,5 @@
+center: [956.2663906909728, 569.2039945552984]
+distort: [-0.25040910859151444, 0.1109210921906881, -0.00041247665414900384, 0.0008205455176671751,
+  -0.026395952816984845]
+focal: [1162.3787554048329, 1162.855381183851]
+skew: 0

GEMstack/knowledge/calibration/make_camera_lidar_yaml.py

@@ -0,0 +1,48 @@
+import yaml
+import numpy as np
+from calib_util import load_ex, load_in
+
+#  
+# THIS FILE SHOULD BE RUN FROM ITS LOCAL DIRECTORY FOR THE PATHS TO WORK
+#
+
+# Destination files name
+output_file = 'gem_e4_perception_cameras.yaml'
+
+# Collect names of all sensors and associated extrinsic/intrinsic files
+camera_files = {'front': ['gem_e4_oak.yaml', 'gem_e4_oak_in.yaml'],
+                'front_right': ['gem_e4_fr.yaml', 'gem_e4_oak_in.yaml'],
+                'front_left': ['gem_e4_fl.yaml', 'gem_e4_fl_in.yaml'],
+                'back_right': ['gem_e4_rr.yaml', 'gem_e4_rr_in.yaml'],
+                'back_left': ['gem_e4_rl.yaml', 'gem_e4_rl_in.yaml']}
+lidar_file = 'gem_e4_ouster.yaml'
+
+# Initialize variables
+output_dict = {'cameras': {}}
+T_lidar_to_vehicle = load_ex(lidar_file, 'matrix')
+
+# Collect data for all cameras
+for camera in camera_files:
+    # Load from files
+    ex_file = camera_files[camera][0]
+    in_file = camera_files[camera][1]
+    T_camera_to_vehicle = load_ex(ex_file, 'matrix')
+    K, D = load_in(in_file, 'matrix', return_distort=True)
+
+    # Calculate necessary values
+    T_lidar_to_camera = np.linalg.inv(T_camera_to_vehicle) @ T_lidar_to_vehicle
+
+    # Store in the proper format
+    camera_dict = {}
+    camera_dict['K'] = K
+    camera_dict['D'] = D
+    camera_dict['T_l2c'] = T_lidar_to_camera
+    camera_dict['T_l2v'] = T_lidar_to_vehicle
+    for key in camera_dict:
+        if type(camera_dict[key]) == np.ndarray:
+            camera_dict[key] = camera_dict[key].tolist()
+    output_dict['cameras'][camera] = camera_dict
+
+# Write to file
+with open(output_file,'w') as stream:
+    yaml.safe_dump(output_dict, stream)

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)
+