From a33fe36e31ad9f67aeb8b3211d3587d4490b2bec Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 23 Feb 2025 09:44:22 -0600 Subject: [PATCH 1/2] Add pedestrain tracking in mpl viz --- GEMstack/onboard/perception/pedestrian_detection.py | 2 +- GEMstack/onboard/visualization/mpl_visualization.py | 9 +++++++++ launch/fixed_route.yaml | 3 +-- launch/pedestrian_detection.yaml | 8 +++++--- 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index e145bcabf..513ff9976 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -239,7 +239,7 @@ 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_agents.clear() # Return if no track results available if track_result[0].boxes.id == None: diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index ef9a24851..931f12775 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -5,6 +5,7 @@ import matplotlib.animation as animation import time from collections import deque +from ...state.agent import AgentEnum class MPLVisualization(Component): """Runs a matplotlib visualization at 10Hz. @@ -71,12 +72,20 @@ def debug_event(self, source, event): plot.popleft() def update(self, state): + # print("State type:", type(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) + + # Add pedestrian tracking + 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) + time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(state.t)) #frame=ObjectFrameEnum.CURRENT #state = state.to_frame(frame) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index c05de8ff7..7f512f10a 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -72,8 +72,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" \ No newline at end of file diff --git a/launch/pedestrian_detection.yaml b/launch/pedestrian_detection.yaml index 1a00fa797..69f8f1c94 100644 --- a/launch/pedestrian_detection.yaml +++ b/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" From 735832fbd390cb22d64d5a13227cd76fb9ee1834 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Mon, 24 Feb 2025 19:27:38 -0600 Subject: [PATCH 2/2] Add feature to separate pedestrain and velocity tracking in mpl visualisation --- .../visualization/mpl_visualization.py | 68 +++++++++++++------ 1 file changed, 46 insertions(+), 22 deletions(-) diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index 90ee37f01..08392e9ed 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -6,6 +6,7 @@ import time from collections import deque from ...state.agent import AgentEnum +import numpy as np class MPLVisualization(Component): """Runs a matplotlib visualization at 10Hz. @@ -22,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 @@ -42,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() @@ -54,37 +59,47 @@ 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): - # print("State type:", type(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) - # Add pedestrian tracking + # 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}", "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 @@ -93,18 +108,27 @@ def update(self, state): 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()