diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index 6c8a73476..f12cec27e 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -15,7 +15,7 @@ Terminal 3: --------------- source /opt/ros/noetic/setup.bash && source /catkin_ws/devel/setup.bash -rosbag play -l ~/rosbags/vehicle.bag +rosbag play -l ~/rosbags/moving_vehicle_stopping_pedestrian.bag --topics /oak/rgb/camera_info /oak/rgb/image_raw /ouster/points /septentrio_gnss/imu /septentrio_gnss/insnavgeod Terminal 4: --------------- @@ -33,7 +33,7 @@ import os from typing import List, Dict from collections import defaultdict -from datetime import datetime +# from datetime import datetime from copy import deepcopy # ROS, CV import rospy @@ -121,7 +121,7 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: self.id_tracker = IdTracker() # Update function variables - self.t_start = datetime.now() # Estimated start frame time + self.t_start = None # Estimated start frame time self.start_pose_abs = None # Get this from GNSS (GLOBAL frame) self.current_vehicle_state = None # Current vehicle state from GNSS in GLOBAL frame self.previous_vehicle_state = None # Previous vehicle state from GNSS in GLOBAL frame @@ -143,6 +143,8 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: # Storing initial pose if (self.start_pose_abs == None): self.start_pose_abs = vehicle.pose + if self.t_start == None: + self.t_start = vehicle.pose.t return self.current_agents else: self.previous_vehicle_state = self.current_vehicle_state @@ -151,6 +153,8 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: if(self.current_agent_obj_dims == {}): return self.current_agents + # Update current time: + self.curr_time = vehicle.pose.t # (f"Global state : {vehicle}") @@ -175,11 +179,23 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: agents = self.create_agent_states(obj_centers=self.current_agent_obj_dims['pose'], obj_dims=self.current_agent_obj_dims['dims']) # Calculating the velocity of agents and tracking their ids - self.find_vels_and_ids(agents=agents) + backOrder = self.find_vels_and_ids(agents=agents) - # Convert to current vehicle frame from starting frame here for planning group. + # Create a dictionary of vehicle frame agents: + vehicle_agents = self.create_vehicle_dict(agents, backOrder) + # Create a list containing the ids of the new agents in the vehicle frame agent's order - return self.current_agents + return vehicle_agents + + def create_vehicle_dict(self, agents: List[AgentState], backOrder: List[int]): + vehicle_agents = {} + + for idx in backOrder: + vehicle_agents[backOrder[idx]] = agents[idx] + vehicle_agents[backOrder[idx]].activity = self.current_agents[backOrder[idx]].activity + vehicle_agents[backOrder[idx]].velocity = self.current_agents[backOrder[idx]].velocity + + return vehicle_agents # TODO: Improve Algo Knn, ransac, etc. def find_centers(self, clusters: List[List[np.ndarray]]) -> List[np.ndarray]: @@ -302,6 +318,7 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L obj_dims = self.find_dims(pedestrians_3d_pts) # 8 point dims of bounding box # Pose is stored in vehicle frame and converted to start frame in the update function obj_centers_vehicle = [] + obj_dims_vehicle = [] if self.vehicle_frame: for obj_center in obj_centers: if obj_center.size > 0: @@ -313,7 +330,6 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L obj_centers = obj_centers_vehicle if(len(obj_center) != 0) and (len(obj_dims) != 0): - obj_dims_vehicle = [] for obj_dim in obj_dims: if len(obj_dim) > 0: # obj_dim_min = np.array([obj_dim[0]]) @@ -336,6 +352,11 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L def find_vels_and_ids(self, agents: List[AgentState]): # self.prev_agents was assigned a deepcopy of self.current_agents and then self.current_agents # was cleared before this function was called + + # Create a list containing the ids of the new agents in the vehicle frame agent's order + backOrder = [] + for each in agents: + backOrder.append(None) # Nothing was scanned, erase current (for current output) and previous list (for next time through because nothing was scanned) if (len(agents) == 0): @@ -359,7 +380,7 @@ def find_vels_and_ids(self, agents: List[AgentState]): vel = [0, 0, 0] else: delta_t = self.curr_time - self.prev_time - vel = list((np.array([agents[idx].pose.x, agents[idx].pose.y, agents[idx].pose.z]) - np.array([prev_state.pose.x, prev_state.pose.y, prev_state.pose.z])) / delta_t.total_seconds()) + vel = list((np.array([agents[idx].pose.x, agents[idx].pose.y, agents[idx].pose.z]) - np.array([prev_state.pose.x, prev_state.pose.y, prev_state.pose.z])) / delta_t) # print("VELOCITY:") # print(vel) @@ -369,6 +390,7 @@ def find_vels_and_ids(self, agents: List[AgentState]): agents[idx].velocity = (0, 0, 0) if len(vel) == 0 else tuple(vel) self.current_agents[prev_id] = agents[idx] + backOrder[idx] = prev_id del self.prev_agents[prev_id] # Remove previous agent from previous agents so it doesn't get assigned multiple times on accident break @@ -380,6 +402,8 @@ def find_vels_and_ids(self, agents: List[AgentState]): agents[idx].activity = AgentActivityEnum.UNDETERMINED agents[idx].velocity = (0, 0, 0) self.current_agents[id] = agents[idx] + backOrder[idx] = id + return backOrder # Calculates whether 2 agents overlap. True if they do, false if not def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool: @@ -426,7 +450,7 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n for idx in range(num_pairings): # Create agent in current frame: agents.append(AgentState( - pose=ObjectPose(t=(self.curr_time - self.t_start).total_seconds(), x=obj_centers[idx][0], y=obj_centers[idx][1], z=obj_centers[idx][2] - xyz_lengths_start[idx][2]/2.0, yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), + pose=ObjectPose(t=self.curr_time - self.t_start, x=obj_centers[idx][0], y=obj_centers[idx][1], z=obj_centers[idx][2] - xyz_lengths_start[idx][2]/2.0, yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), # Beware: AgentState(PhysicalObject) builds bbox from # dims [-l/2,l/2] x [-w/2,w/2] x [0,h], not # [-l/2,l/2] x [-w/2,w/2] x [-h/2,h/2] @@ -435,10 +459,9 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n dimensions=(xyz_lengths_start[idx][0], xyz_lengths_start[idx][1], xyz_lengths_start[idx][2]), outline=None, type=AgentEnum.PEDESTRIAN, - activity=AgentActivityEnum.UNDETERMINED, # TODO - velocity=None, # TODO + activity=AgentActivityEnum.UNDETERMINED, # Temporary + velocity=None, # Temporary yaw_rate=0, - track_id = None # TODO )) return agents @@ -502,7 +525,7 @@ def ouster_oak_callback(self, cv_image: cv2.Mat, lidar_points: np.ndarray): # Update times for basic velocity calculation self.prev_time = self.curr_time - self.curr_time = datetime.now() + # self.curr_time = datetime.now() # Updating in update function now self.cv_image = cv_image self.lidar_points = lidar_points