Merge branch 's2025_teamB' into teamB_control_planning

Author: brijesh2709

GEMstack/onboard/perception/pedestrian_detection.py

@@ -319,6 +319,8 @@ def viz_object_states(self, cv_image, boxes, extracted_pts_all):
 
 
     def update_object_states(self, track_result: List[Results], extracted_pts_all: List[np.ndarray]) -> None:
+        # self.prev_agents = self.current_agents.copy()
+        # self.current_agents.clear()
 
         # self.current_agent_obj_dims.clear()
         # Return if no track results available

GEMstack/onboard/visualization/mpl_visualization.py

@@ -5,6 +5,8 @@
 import matplotlib.animation as animation
 import time
 from collections import deque
+from ...state.agent import AgentEnum
+import numpy as np
 
 class MPLVisualization(Component):
     """Runs a matplotlib visualization at 10Hz. 
@@ -21,8 +23,12 @@ def __init__(self, rate : float = 10.0, save_as : str = None):
         self.axs = None
         self.tstart = 0
         self.plot_t_range = 10
-        self.plot_values = {}
-        self.plot_events = {}
+
+        # Separate vehicle and pedestrian tracking
+        self.vehicle_plot_values = {}
+        self.pedestrian_plot_values = {}
+        self.vehicle_plot_events = {}
+        self.pedestrian_plot_events = {}
 
     def rate(self) -> float:
         return self._rate
@@ -41,7 +47,7 @@ def initialize(self):
             self.writer.setup(plt.gcf(), self.save_as, dpi=100)
         plt.ion()
         # to run GUI event loop
-        self.fig,self.axs = plt.subplots(1,2,figsize=(12,6))
+        self.fig,self.axs = plt.subplots(1,3,figsize=(18,6))
         self.fig.canvas.mpl_connect('close_event', self.on_close)
         plt.show(block=False)
         self.tstart = time.time()
@@ -53,49 +59,76 @@ def on_close(self,event):
     def debug(self, source, item, value):
         t = time.time() - self.tstart
         item = source+'.'+item
-        if item not in self.plot_values:
-            self.plot_values[item] = deque()
-        plot = self.plot_values[item]
-        self.plot_values[item].append((t,value))
+        # Determine which plot dict to use based on source
+        if source.startswith('ped_'):
+            target_dict = self.pedestrian_plot_values
+        else:
+            target_dict = self.vehicle_plot_values
+            
+        if item not in target_dict:
+            target_dict[item] = deque()
+        plot = target_dict[item]
+        plot.append((t,value))
         while t - plot[0][0] > self.plot_t_range:
             plot.popleft()
 
     def debug_event(self, source, event):
         t = time.time() - self.tstart
         event = source+'.'+event
-        if event not in self.plot_events:
-            self.plot_events[event] = deque()
-        plot = self.plot_events[event]
+        target_dict = self.pedestrian_plot_events if source.startswith('ped_') else self.vehicle_plot_events
+        
+        if event not in target_dict:
+            target_dict[event] = deque()
+        plot = target_dict[event]
         plot.append(t)
         while t - plot[0] > self.plot_t_range:
             plot.popleft()
 
     def update(self, state):
         if not plt.fignum_exists(self.fig.number):
-            #plot closed
             return
         self.num_updates += 1
-        self.debug("vehicle","velocity",state.vehicle.v)
-        self.debug("vehicle","front wheel angle",state.vehicle.front_wheel_angle)        
+        
+        # Vehicle metrics
+        self.debug("vehicle", "velocity", state.vehicle.v)
+        self.debug("vehicle", "front_wheel_angle", state.vehicle.front_wheel_angle)
+        
+        # Pedestrian metrics
+        for agent_id, agent in state.agents.items():
+            if agent.type == AgentEnum.PEDESTRIAN:
+                # self.debug(f"ped_{agent_id}", "x", agent.pose.x)
+                # self.debug(f"ped_{agent_id}", "y", agent.pose.y)
+                self.debug(f"ped_{agent_id}", "velocity", np.linalg.norm(agent.velocity))  # Magnitude of resultant velocity
+                self.debug(f"ped_{agent_id}", "yaw_rate", agent.yaw_rate)
+        
         time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(state.t))
         #frame=ObjectFrameEnum.CURRENT
         #state = state.to_frame(frame)
         xrange = [state.vehicle.pose.x - 10, state.vehicle.pose.x + 10]
         yrange = [state.vehicle.pose.y - 10, state.vehicle.pose.y + 10]
         #plot main visualization
         mpl_visualization.plot(state,title="Scene %d at %s"%(self.num_updates,time_str),xrange=xrange,yrange=yrange,show=False,ax=self.axs[0])
-        #plot figure on axs[1]
+        
+        # Vehicle plot (axs[1])
         self.axs[1].clear()
-        for k,v in self.plot_values.items():
+        for k,v in self.vehicle_plot_values.items():
             t = [x[0] for x in v]
             y = [x[1] for x in v]
             self.axs[1].plot(t,y,label=k)
-        for i,(k,v) in enumerate(self.plot_events.items()):
-            for t in v:
-                self.axs[1].axvline(x=t,linestyle='--',color='C'+str(i),label=k)
+        self.axs[1].set_title('Vehicle Metrics')
         self.axs[1].set_xlabel('Time (s)')
         self.axs[1].legend()
 
+        # Pedestrian plot (axs[2])
+        self.axs[2].clear()
+        for k,v in self.pedestrian_plot_values.items():
+            t = [x[0] for x in v]
+            y = [x[1] for x in v]
+            self.axs[2].plot(t,y,label=k)
+        self.axs[2].set_title('Pedestrian Metrics')
+        self.axs[2].set_xlabel('Time (s)')
+        self.axs[2].legend()
+
         self.fig.canvas.draw_idle()
         self.fig.canvas.flush_events()
 

launch/fixed_route.yaml

@@ -74,8 +74,7 @@ variants:
                 perception:
                     state_estimation : OmniscientStateEstimator
                     agent_detection : OmniscientAgentDetector
-            visualization: !include "klampt_visualization.yaml"
-            #visualization: !include "mpl_visualization.yaml"
+            visualization: [!include "klampt_visualization.yaml", !include "mpl_visualization.yaml"]
     log_ros:
         log:
             ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml"

launch/pedestrian_detection.yaml

@@ -57,9 +57,11 @@ variants:
     detector_only:
         run:
             description: "Run the pedestrian detection code"
-        drive:
-            planning:
-                trajectory_tracking:
+            drive:
+                planning:
+                    trajectory_tracking:
+            visualization: !include "mpl_visualization.yaml"
+
     real_sim:
         run:
             description: "Run the pedestrian detection code with real detection and fake simulation"