Merge branch 's2025_Simulation' into gazebo_agent_detection

Author: harishkumarbalaji

GEMstack/knowledge/defaults/__init__.py

@@ -1,36 +1,7 @@
 from ...utils.config import load_config_recursive
-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)
+import os
+SETTINGS_FILE = os.path.join(os.path.split(__file__)[0],'current.yaml')
 print("**************************************************************")
-print(f"Loading {vehicle} settings from",SETTINGS_FILE)
+print("Loading default settings from",SETTINGS_FILE)
 print("**************************************************************")
 SETTINGS = load_config_recursive(SETTINGS_FILE)

GEMstack/knowledge/detection/yolov8n.pt

@@ -72,11 +72,11 @@ def point_segment_distance(x,a,b) -> Tuple[float,float]:
     udotv = np.dot(u,v)
     if udotv < 0:
         return vector_norm(u),0
-    elif udotv > vnorm:
+    elif udotv > vnorm**2:
         return vector_norm(vector_sub(x,b)),1
     else:
-        param = udotv/vnorm
-        return vector_norm(vector_sub(u,vector_mul(v,param/vnorm))),param
+        param = udotv/vnorm**2
+        return vector_norm(vector_sub(u,vector_mul(v,param))),param
 
 def rotate2d(point, angle : float, origin=None):
     """Rotates a point about the origin by an angle"""

GEMstack/mathutils/transforms.py

@@ -1,3 +1,16 @@
+"""
+Demo script to run YOLO object detection on a Gazebo simulation.
+
+This code subscribes to the front camera feed from the GEM e2/e4 model and applies YOLO-based object detection to the incoming images.
+
+Visualization:
+- Use RViz or rqt to monitor the topics.
+
+ROS Topics:
+- Raw camera feed: /gem/debug
+- YOLO detection output: /gem/image_detection
+"""
+
 
 # Python 
 import os
@@ -26,41 +39,45 @@
 
 
 class ObjectDetection(Component):
-    # TODO: Pull params into a JSON/yaml
-    # TODO: Convert some lists into np.arrays, vectorize calculations
-    # TODO: Implement logging instead of print, cleanup comments
-    # TODO: Cleanup funcs + split into separate classes
-    # TODO: Decide if we want to name dets "peds" or "objs"/"agents"
-    #       Maybe peds for now and Agents in agent_detection.py?
-    def __init__(self, vehicle_interface : GEMInterface) -> None:
 
+    def __init__(self, vehicle_interface : GEMInterface) -> None:
+        """
+        Initializes essential functions required to run the YOLO model.
+        """
 
         # vehicle interface
         self.vehicle_interface = vehicle_interface
-
-
         # cv2 to ros converter
         self.bridge = CvBridge()
 
-
         # yolo model
         self.detector = YOLO(os.getcwd()+'/GEMstack/knowledge/detection/yolov8n.pt')
         self.confidence = 0.1
         self.classes_to_detect = 0
 
 
     def initialize(self):
+        """
+        Defines callback functions for subscribing to the front camera image stream and sets up publishers for debugging purposes.
+        """
+
         super().initialize()
         self.vehicle_interface.subscribe_sensor('front_camera',self.front_camera_callback, type = cv2.Mat)
-        # self.pub_pedestrians_pc2 = rospy.Publisher("/point_cloud/pedestrians", PointCloud2, queue_size=10)
-        # self.pub_obj_centers_pc2 = rospy.Publisher("/point_cloud/obj_centers", PointCloud2, queue_size=10)
-        # self.pub_bboxes_markers = rospy.Publisher("/markers/bboxes", MarkerArray, queue_size=10)
         self.pub_image = rospy.Publisher("/gem/debug", Image, queue_size=1)
         self.pub_detimage = rospy.Publisher("/gem/image_detection", Image, queue_size=1)
 
 
     def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
 
+        """
+        Displays vehicle statistics in the GLOBAL reference frame.
+
+        This function also allows switching between coordinate frames using the `VehicleState -> pose` method.
+
+        Returning an `AgentState` will automatically log detected objects.
+        """
+
+
         print(f"VEHICLE State at time: {vehicle.pose.t}")
 
         print(f"x: {vehicle.pose.x}")
@@ -79,6 +96,10 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
 
     def front_camera_callback(self, image: cv2.Mat):
 
+        """
+        A simple callback function that processes incoming images, runs them through a YOLO model, and visualizes the detections.
+        """
+
 
         ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
         self.pub_image.publish(ros_img)
@@ -132,10 +153,6 @@ def front_camera_callback(self, image: cv2.Mat):
 
         ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
         self.pub_detimage.publish(ros_img)
-        
-
-        
-
 
 
     def rate(self):

…k/onboard/perception/object_detection.py …erception/test_yolo_gazebo_simulation.pyGEMstack/onboard/perception/object_detection.py renamed to GEMstack/onboard/perception/test_yolo_gazebo_simulation.py GEMstack/onboard/perception/object_detection.py renamed to GEMstack/onboard/perception/test_yolo_gazebo_simulation.py

@@ -1,23 +1,33 @@
 import json
-from ..knowledge import defaults
 import copy
 from typing import List,Union,Any
 
 SETTINGS = None
 
-def load_settings():
+def load_settings(settings_file : str = None):
     """Loads the settings object for the first time.
     
-    Order of operations is to look into defaults.SETTINGS, and then
-    look through the command line arguments to determine whether the user has
-    overridden any settings using --KEY=VALUE.
+    Order of operations is:
+    - If settings_file is given, load it.
+    - Otherwise, get the settings from defaults.SETTINGS
+    - Look through the command line arguments to determine whether the user has
+      overridden any settings using --KEY=VALUE.
     """
     global SETTINGS
     if SETTINGS is not None:
         return
     import os
     import sys
-    SETTINGS = copy.deepcopy(defaults.SETTINGS)
+    if settings_file is not None:
+        from .config import load_config_recursive
+        import os
+        print("**************************************************************")
+        print("Loading global settings from",settings_file)
+        print("**************************************************************")
+        SETTINGS = load_config_recursive(os.path.abspath(settings_file))
+    else:
+        from ..knowledge import defaults
+        SETTINGS = copy.deepcopy(defaults.SETTINGS)
     for arg in sys.argv:
         if arg.startswith('--'):
             k,v = arg.split('=',1)

GEMstack/utils/settings.py

@@ -68,15 +68,16 @@ In one terminal, run the Gazebo simulator (using the instructions provided in th
 Open a second terminal and launch GEMStack with your configured launch file.
 
 ```bash
-python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/{your_file}.yaml
+python3 main.py --variant=gazebo --settings={your_file}.yaml launch/{your_file}.yaml
+
 ```
 - Make sure to set the variant to `gazebo`.
-- You can specify the vehicle type to be `e2_gazebo` or `e4_gazebo`.
+- You can specify the settings file to  GEMstack/knowledge/defaults/e4_gazebo.yaml or e2_gazebo.yaml till we work on closing the sim to real gap.
 
 Example command launching the fixed route with e4 vehicle:
 
 ```bash
-python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/fixed_route.yaml
+python3 main.py --variant=gazebo --settings=GEMstack/knowledge/defaults/e4_gazebo.yaml launch/fixed_route.yaml
 ```
 **Variants:**
  - sim
@@ -87,6 +88,18 @@ python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/fixed_route.yaml
 - e4
 - e2_gazebo
 - e4_gazebo
+
+**Setting:**
+
+By default it takes GEMstack/knowledge/defaults/current.yaml which is GEM E4 Vehicle configuration settings.
+
+Other available configuration files:
+
+GEMstack/knowledge/defaults/
+- e2.yaml
+- e2_gazebo.yaml
+- e4_gazebo.yaml
+
 ---
 
 ## Agent Detection in Gazebo

docs/Gazebo Simulation Documentation.md

@@ -72,7 +72,7 @@ variants:
             drive: 
                 perception:
                     state_estimation : GNSSStateEstimator
-                    agent_detection : object_detection.ObjectDetection
+                    agent_detection : test_yolo_gazebo_simulation.ObjectDetection
                     perception_normalization : StandardPerceptionNormalizer
 
                 planning: