Add pedestrain tracking in mpl viz

Author: harishkumarbalaji

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:

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)

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"

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"