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

Author: brijesh2709

GEMstack/knowledge/calibration/cameras.yaml

@@ -1,35 +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 ]
-
+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/defaults/computation_graph.yaml

@@ -61,12 +61,24 @@ components:
   - driving_logic:
       inputs: 
       outputs: intent
+<<<<<<< HEAD
+  - parking_component: # one way
+      inputs: all
+      outputs: mission_plan
+  - summon_component: # one way
+      inputs: all
+      outputs: route
+  - route_planning_component: # one way
+      inputs: [vehicle,agents,mission_plan, obstacles]
+      outputs: route
+=======
   - _mission_planner: # one way
       inputs: all
       outputs: mission_plan
   - _route_planner: # one way
       inputs: all
       outputs: route
+>>>>>>> c2672669e19024ddf6f0ec39238970f2bbca80ed
   - motion_planning:
       inputs: all
       outputs: trajectory
@@ -75,4 +87,8 @@ components:
       outputs: 
   - signaling:
       inputs: [intent]
-      outputs:
+<<<<<<< HEAD
+      outputs:
+=======
+      outputs:
+>>>>>>> c2672669e19024ddf6f0ec39238970f2bbca80ed

GEMstack/onboard/execution/execution.py

@@ -609,7 +609,12 @@ def validate_sensors(self,numsteps=None):
 
             #check for vehicle faults
             self.check_for_hardware_faults()
-
+            for name,c in perception_components.items():
+                if not c.healthy():
+                    if c.essential:
+                        executor_debug_print(1,"Sensor {} not working, entering recovery mode",name)
+                    else:
+                        executor_debug_print(1,"Warning, sensor {} not working, ignoring",name)
             self.update_components(perception_components,self.state)
             sensors_working = all([c.healthy() for c in perception_components.values()])
 

GEMstack/onboard/interface/gem_gazebo.py

@@ -223,6 +223,7 @@ def model_states_callback(self, msg: ModelStates):
             # Check if this model should be tracked as an agent or obstacle
             agent_type = None
             for prefix in self.tracked_model_prefixes:
+
                 if model_name.lower().startswith(prefix.lower()):
                     for key, value in MODEL_PREFIX_TO_AGENT_TYPE.items():
                         if prefix.lower().startswith(key.lower()):

GEMstack/onboard/perception/agent_detection.py

@@ -13,7 +13,7 @@ def __init__(self,vehicle_interface : GEMInterface):
         self.lock = threading.Lock()
 
     def rate(self):
-        return 4.0
+        return 10.0
 
     def state_inputs(self):
         return []
@@ -25,6 +25,7 @@ def initialize(self):
         self.vehicle_interface.subscribe_sensor('agent_detector',self.agent_callback, AgentState)
 
     def agent_callback(self, name : str, agent : AgentState):
+        print(f"AGENT detected: {name}")
         with self.lock:
             self.agents[name] = agent
 

GEMstack/onboard/perception/state_estimation.py

@@ -15,6 +15,7 @@ class GNSSStateEstimator(Component):
     """Just looks at the GNSS reading to estimate the vehicle state"""
     def __init__(self, vehicle_interface : GEMInterface):
         self.vehicle_interface = vehicle_interface
+        print("GNSSStateEstimator", vehicle_interface.sensors())
         if 'gnss' not in vehicle_interface.sensors():
             raise RuntimeError("GNSS sensor not available")
         vehicle_interface.subscribe_sensor('gnss',self.gnss_callback,GNSSReading)
@@ -26,6 +27,7 @@ def __init__(self, vehicle_interface : GEMInterface):
 
     # Get GNSS information
     def gnss_callback(self, reading : GNSSReading):
+        # print("GNSS callback",reading)
         self.gnss_pose = reading.pose
         self.gnss_speed = reading.speed
         self.status = reading.status

GEMstack/onboard/perception/test_yolo_gazebo_simulation.py

@@ -118,36 +118,37 @@ def front_camera_callback(self, image: cv2.Mat):
 
         boxes = track_result[0].boxes
         for box in boxes:
-
+            # print("BOXES: ", box)   
 
             class_id = int(box.cls.item())
             label_text = class_names[class_id]
             xywh = box.xywh[0].tolist()
             x, y, w, h = xywh
-            id = box.id.item()
+            if box.id!=None:
+                id = box.id.item()
 
-            # Draw bounding box
-            cv2.rectangle(image, (int(x - w / 2), int(y - h / 2)), (int(x + w / 2), int(y + h / 2)), (255, 0, 255), 3)
+                # Draw bounding box
+                cv2.rectangle(image, (int(x - w / 2), int(y - h / 2)), (int(x + w / 2), int(y + h / 2)), (255, 0, 255), 3)
 
-            # Define text label
-            x = int(x - w / 2)
-            y = int(y - h / 2)
-            label = label_text + str(id) + " : " + str(round(box.conf.item(), 2))
+                # Define text label
+                x = int(x - w / 2)
+                y = int(y - h / 2)
+                label = label_text + str(id) + " : " + str(round(box.conf.item(), 2))
 
-            # Get text size
-            text_size, baseline = cv2.getTextSize(label, font, font_scale, line_type)
-            text_w, text_h = text_size
+                # Get text size
+                text_size, baseline = cv2.getTextSize(label, font, font_scale, line_type)
+                text_w, text_h = text_size
 
-            # Position text above the bounding box
-            text_x = x
-            text_y = y - 10 if y - 10 > 10 else y + h + text_h
+                # Position text above the bounding box
+                text_x = x
+                text_y = y - 10 if y - 10 > 10 else y + h + text_h
 
-            # Draw text outline for better visibility
-            for dx, dy in [(-1, -1), (-1, 1), (1, -1), (1, 1)]:  
-                cv2.putText(image, label, (text_x + dx, text_y - baseline + dy), font, font_scale, outline_color, outline_thickness)
+                # Draw text outline for better visibility
+                for dx, dy in [(-1, -1), (-1, 1), (1, -1), (1, 1)]:  
+                    cv2.putText(image, label, (text_x + dx, text_y - baseline + dy), font, font_scale, outline_color, outline_thickness)
 
-            # Draw main text on top of the outline
-            cv2.putText(image, label, (text_x, text_y - baseline), font, font_scale, font_color, text_thickness)
+                # Draw main text on top of the outline
+                cv2.putText(image, label, (text_x, text_y - baseline), font, font_scale, font_color, text_thickness)
 
 
 

GEMstack/onboard/planning/collision.py

@@ -1,4 +1,3 @@
-import numpy as np
 from scipy.ndimage import distance_transform_edt
 
 def build_collision_lookup(grid, safety_margin=2, vehicle_width=1.0):
@@ -21,7 +20,6 @@ def build_collision_lookup(grid, safety_margin=2, vehicle_width=1.0):
         "margin": margin,
     }
 
-
 def fast_collision_check(x, y, lookup):
     """
     Check if a point is in collision using the precomputed lookup table.
@@ -39,7 +37,6 @@ def fast_collision_check(x, y, lookup):
         return True  # Out of bounds is considered collision
     return cm[xi, yi]
 
-
 def path_collision_check(path, lookup, x_off=0, y_off=0):
     """
     Check if a path is collision-free.