diff --git a/GEMstack/knowledge/calibration/gem_e4.yaml b/GEMstack/knowledge/calibration/gem_e4.yaml index da971c253..d0954dc06 100644 --- a/GEMstack/knowledge/calibration/gem_e4.yaml +++ b/GEMstack/knowledge/calibration/gem_e4.yaml @@ -1,7 +1,7 @@ calibration_date: "2024-03-05" # Date of calibration YYYY-MM-DD reference: rear_axle_center # rear axle center rear_axle_height: 0.33 # height of rear axle center above flat ground -gnss_location: [1.10,0.1.62] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py. Note conflict with pure pursuit location? +gnss_location: [1.10,0,1.62] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py. Note conflict with pure pursuit location? gnss_yaw: 0.0 # radians top_lidar: !include "gem_e4_ouster.yaml" front_camera: !include "gem_e4_oak.yaml" diff --git a/GEMstack/mathutils/dubins.py b/GEMstack/mathutils/dubins.py index cdb4655ab..b840201d9 100644 --- a/GEMstack/mathutils/dubins.py +++ b/GEMstack/mathutils/dubins.py @@ -44,7 +44,7 @@ def derivative(self,x,u): right = [-fwd[1],fwd[0]] phi = u[1] d = u[0] - return np.array([fwd[0]*d,fwd[1]*d,phi]) + return np.array([fwd[0]*d,fwd[1]*d,phi*d]) class DubinsCarIntegrator(ControlSpace): diff --git a/GEMstack/mathutils/transforms.py b/GEMstack/mathutils/transforms.py index 833ecc80d..a29ec48e2 100644 --- a/GEMstack/mathutils/transforms.py +++ b/GEMstack/mathutils/transforms.py @@ -36,6 +36,14 @@ def vector_dist(v1, v2) -> float: """Euclidean distance between two vectors""" return vo.distance(v1,v2) +def vector_dot(v1, v2) -> float: + """Dot product between two vectors""" + return vo.dot(v1,v2) + +def vector_cross(v1, v2) -> float: + """Cross product between two 2D vectors""" + return vo.cross(v1,v2) + def vector2_angle(v1, v2 = None) -> float: """find the ccw angle bewtween two 2d vectors""" if v2 is None: @@ -123,3 +131,16 @@ def xy_to_lat_lon(x_east : float, y_north : float, lat_reference : float, lon_re # convert GNSS waypoints into local fixed frame reprented in x and y lat, lon = axy.xy2ll(x_east, y_north, lat_reference, lon_reference) return lat, lon + +def quaternion_to_euler(x : float, y : float, z : float, w : float): + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll = np.arctan2(t0, t1) + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch = np.arcsin(t2) + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw = np.arctan2(t3, t4) + return [roll, pitch, yaw] diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..e268d4b47 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -1,11 +1,13 @@ from .gem import * from ...utils import settings import math +import time # ROS Headers import rospy from std_msgs.msg import String, Bool, Float32, Float64 from sensor_msgs.msg import Image,PointCloud2 +import message_filters try: from novatel_gps_msgs.msg import NovatelPosition, NovatelXYZ, Inspva except ImportError: @@ -55,6 +57,7 @@ def __init__(self): self.front_depth_sub = None self.top_lidar_sub = None self.stereo_sub = None + self.sync = None self.faults = [] # -------------------- PACMod setup -------------------- @@ -151,6 +154,7 @@ def subscribe_sensor(self, name, callback, type = None): if name == 'gnss': topic = self.ros_sensor_topics[name] if topic.endswith('inspva'): + #GEM e2 uses Novatel GNSS if type is not None and (type is not Inspva and type is not GNSSReading): raise ValueError("GEMHardwareInterface GEM e2 only supports Inspva/GNSSReading for GNSS") if type is Inspva: @@ -169,7 +173,7 @@ def callback_with_gnss_reading(inspva_msg: Inspva): callback(GNSSReading(pose,speed,inspva_msg.status)) self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) else: - #assume it's septentrio + #assume it's septentrio on GEM e4 if type is not None and (type is not INSNavGeod and type is not GNSSReading): raise ValueError("GEMHardwareInterface GEM e4 only supports INSNavGeod/GNSSReading for GNSS") if type is INSNavGeod: @@ -177,8 +181,9 @@ def callback_with_gnss_reading(inspva_msg: Inspva): else: def callback_with_gnss_reading(msg: INSNavGeod): pose = ObjectPose(ObjectFrameEnum.GLOBAL, - x=msg.longitude, - y=msg.latitude, + t=self.time(), + x=math.degrees(msg.longitude), #Septentrio GNSS uses radians rather than degrees + y=math.degrees(msg.latitude), z=msg.height, yaw=math.radians(msg.heading), #heading from north in degrees (TODO: maybe?? check this) roll=math.radians(msg.roll), @@ -186,7 +191,7 @@ def callback_with_gnss_reading(msg: INSNavGeod): ) speed = np.sqrt(msg.ve**2 + msg.vn**2) callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) - self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) + self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) elif name == 'top_lidar': topic = self.ros_sensor_topics[name] if type is not None and (type is not PointCloud2 and type is not np.ndarray): @@ -227,6 +232,40 @@ def callback_with_cv2(msg : Image): cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough") callback(cv_image) self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2) + elif name == 'sensor_fusion_Lidar_Camera_GNSS': + def callback_with_cv2_numpy_gnss(rgb_image_msg: Image, lidar_pc2_msg: PointCloud2, gnss_msg: INSNavGeod): + points = conversions.ros_PointCloud2_to_numpy(lidar_pc2_msg, want_rgb=False) + cv_image = conversions.ros_Image_to_cv2(rgb_image_msg, desired_encoding="bgr8") + pose = ObjectPose(ObjectFrameEnum.GLOBAL, + t = 0, + x=gnss_msg.longitude, + y=gnss_msg.latitude, + z=gnss_msg.height, + yaw=math.radians(gnss_msg.heading), #heading from north in degrees (TODO: maybe?? check this) + roll=math.radians(gnss_msg.roll), + pitch=math.radians(gnss_msg.pitch), + ) + speed = np.sqrt(gnss_msg.ve**2 + gnss_msg.vn**2) + callback(cv_image,points,GNSSReading(pose,speed,('error' if gnss_msg.error else 'ok'))) + topic_camera = self.ros_sensor_topics['front_camera'] + topic_lidar = self.ros_sensor_topics['top_lidar'] + topic_gnss = self.ros_sensor_topics['gnss'] + self.front_camera_sub = message_filters.Subscriber(topic_camera, Image) + self.top_lidar_sub = message_filters.Subscriber(topic_lidar, PointCloud2) + self.gnss_sub = message_filters.Subscriber(topic_gnss, INSNavGeod) + self.sync = message_filters.ApproximateTimeSynchronizer([self.front_camera_sub, self.top_lidar_sub,self.gnss_sub], queue_size=10, slop=0.1) + self.sync.registerCallback(callback_with_cv2_numpy_gnss) + elif name == 'sensor_fusion_Lidar_Camera': + def callback_with_cv2_numpy(rgb_image_msg: Image, lidar_pc2_msg: PointCloud2): + points = conversions.ros_PointCloud2_to_numpy(lidar_pc2_msg, want_rgb=False) + cv_image = conversions.ros_Image_to_cv2(rgb_image_msg, desired_encoding="bgr8") + callback(cv_image,points) + topic_camera = self.ros_sensor_topics['front_camera'] + topic_lidar = self.ros_sensor_topics['top_lidar'] + self.front_camera_sub = message_filters.Subscriber(topic_camera, Image) + self.top_lidar_sub = message_filters.Subscriber(topic_lidar, PointCloud2) + self.sync = message_filters.ApproximateTimeSynchronizer([self.front_camera_sub, self.top_lidar_sub], queue_size=10, slop=0.1) + self.sync.registerCallback(callback_with_cv2_numpy) # PACMod enable callback function diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index ee041d3a2..c2f8c58a8 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -48,9 +48,11 @@ # GEMStack from ...state import AllState,VehicleState,ObjectPose,ObjectFrameEnum,AgentState,AgentEnum,AgentActivityEnum,ObjectFrameEnum from .pedestrian_detection_utils import * -from ..interface.gem import GEMInterface +from ..interface.gem import GEMInterface, GNSSReading from ..component import Component from .IdTracker import IdTracker +from scipy.spatial.transform import Rotation as R +from septentrio_gnss_driver.msg import INSNavGeod def box_to_fake_agent(box): """Creates a fake agent state from an (x,y,w,h) bounding box. @@ -81,6 +83,7 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: # track_id: AgentState self.prev_agents = dict() self.current_agents = dict() + self.current_agent_obj_dims = dict() # TODO Implement time self.confidence = 0.7 self.classes_to_detect = 0 @@ -99,11 +102,23 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: [-0.02289521, 0.00333035, 0.9997323]]) self.t_lidar_to_vehicle = np.array([[0.0], [0.0], [0.35]]) - # Subscribers and sychronizers - self.rgb_rosbag = message_filters.Subscriber('/oak/rgb/image_raw', Image) - self.top_lidar_rosbag = message_filters.Subscriber('/ouster/points', PointCloud2) - self.sync = message_filters.ApproximateTimeSynchronizer([self.rgb_rosbag, self.top_lidar_rosbag], queue_size=10, slop=0.1) - self.sync.registerCallback(self.ouster_oak_callback) + # To calculate vehicle to start frame data + self.t_start_to_world = None + self.t_vehicle_to_world = None + self.t_vehicle_to_start = None + + # Rospy Subscribers and sychronizers + # self.rgb_rosbag = message_filters.Subscriber('/oak/rgb/image_raw', Image) + # self.top_lidar_rosbag = message_filters.Subscriber('/ouster/points', PointCloud2) + # self.sync = message_filters.ApproximateTimeSynchronizer([self.rgb_rosbag, self.top_lidar_rosbag], queue_size=10, slop=0.1) + # self.sync.registerCallback(self.ouster_oak_callback) + + # GEMStack Subscribers and sychronizers + # LIDAR Camera fusion + self.vehicle_interface.subscribe_sensor('sensor_fusion_Lidar_Camera',self.ouster_oak_callback) + + # LIDAR Camera GNSS fusion + # self.vehicle_interface.subscribe_sensor('sensor_fusion_Lidar_Camera_GNSS',self.ouster_oak_callback) # TF listener to get transformation from LiDAR to Camera self.tf_listener = tf.TransformListener() @@ -116,14 +131,9 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: # Update function variables self.t_start = datetime.now() # Estimated start frame time - # self.start_pose_abs = None - # self.start_pose_abs = ObjectPose( - # frame=ObjectFrameEnum.GLOBAL, - # t=0, - # x=0, - # y=0, - # z=0 - # ) + 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 def init_debug(self,) -> None: # Debug Publishers @@ -132,11 +142,67 @@ def init_debug(self,) -> None: self.pub_bboxes_markers = rospy.Publisher("/markers/bboxes", MarkerArray, queue_size=10) self.pub_image = rospy.Publisher("/camera/image_detection", Image, queue_size=1) + + # Test code to check gnss , can be removed + # Debugging + def gnss_test(self, cv_image: cv2.Mat, lidar_points: np.ndarray, vehicle_state: GNSSReading): + print(f"vehicle global state: {vehicle_state}") + + def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: - # if self.start_pose_abs is None: - # self.start_pose_abs = vehicle.pose # Store first pose which is start frame - # self.start_pose_abs.to_frame(frame=ObjectFrameEnum.GLOBAL, current_pose=) - # self.current_vehicle_pose_sf = vehicle.pose # Vehicle pose in start frame + + # Edge cases + + if(self.current_vehicle_state == None and self.previous_vehicle_state == None): + self.current_vehicle_state = vehicle + # We get vehicle state from GNSS in global state + # Storing initial pose + if (self.start_pose_abs == None): + self.start_pose_abs = vehicle.pose + return self.current_agents + else: + self.previous_vehicle_state = self.current_vehicle_state + self.current_vehicle_state = vehicle + + if(self.current_agent_obj_dims == {}): + return self.current_agents + + print(f"Global state : {vehicle}") + + # Convert pose to start state. Need to use previous_vehicle state as pedestrian info is delayed + vehicle_start_pose = vehicle.pose.to_frame(ObjectFrameEnum.START,self.previous_vehicle_state.pose,self.start_pose_abs) + print(f"Start state : {vehicle_start_pose}") + + print(f"ped pose vehicle state = {self.current_agent_obj_dims['pose']}") + print(f"ped dimenstions vehicle state = {self.current_agent_obj_dims['dims']}") + + # converting to start frame + for i,pose in enumerate(self.current_agent_obj_dims['pose']): + self.current_agent_obj_dims['pose'][i] = np.array(vehicle_start_pose.apply(pose)) + + for i,dims in enumerate(self.current_agent_obj_dims['dims']): + self.current_agent_obj_dims['dims'][i] = np.array(vehicle_start_pose.apply(dims)) + + + print(f"ped pose start state = {self.current_agent_obj_dims['pose']}") + print(f"ped dimenstions start state = {self.current_agent_obj_dims['dims']}") + + # Prepare variables for find_vels_and_ids + self.prev_agents = self.current_agents.copy() + self.current_agents.clear() + # Note this below function will probably throw a type error. I think we'll need to index the + # tuples by index 0 in the lists but I'm not sure: + 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) + + # Convert to current vehicle frame here for planning group. + # Create a new dictionary here which is a deepcopy of self.current_agents + # convert this new dictionary to current vehicle frame + # return this new dictionary + # OR just make planning group do the conversion. Depends on what data we want to log? + return self.current_agents # TODO: Improve Algo Knn, ransac, etc. @@ -155,29 +221,6 @@ def find_dims(self, clusters: List[List[np.ndarray]]) -> List[np.ndarray]: clusters = [np.array(clust) for clust in clusters] dims = [np.array(()) if clust.size == 0 else np.max(clust, axis= 0) - np.min(clust, axis= 0) for clust in clusters] return dims - - # TODO: Slower but cleaner to input self.current_agents dict - # TODO: Moving Average across last N iterations pos/vel? Less spurious vals - # TODO Akul: Fix velocity calculation to calculate in ObjectFrameEnum.START - # Work towards own tracking class instead of simple YOLO track? - # Fix 1: division by time - # Fix 2: Put centers and dims in start frame for velocity calc + final agentstate in update_object_states - # ret: Dict[track_id: vel[x, y, z]] - # def find_vels(self, track_ids: List[int], obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> Dict[int, np.ndarray]: - # # Object not seen -> velocity = None - # track_id_center_map = dict(zip(track_ids, obj_centers)) - # vels = defaultdict(lambda: np.array(())) # None is faster, np.array matches other find_ methods. - - # for prev_track_id, prev_agent in self.prev_agents.items(): - # if prev_track_id in track_ids: - # # TODO: Add prev_agents to memory to avoid None velocity - # # We should only be missing prev pose on first sight of track_id Agent. - # # print("shape 1: ", track_id_center_map[prev_agent.track_id]) - # # print("shape 2: ", np.array([prev_agent.pose.x, prev_agent.pose.y, prev_agent.pose.z])) - # # prev can be 3 separate Nones, current is just empty array... make this symmetrical - # if prev_agent.pose.x and prev_agent.pose.y and prev_agent.pose.z and track_id_center_map[prev_agent.track_id].shape == 3: - # vels[prev_track_id] = (track_id_center_map[prev_track_id] - np.array([prev_agent.pose.x, prev_agent.pose.y, prev_agent.pose.z])) / (self.curr_time - self.prev_time) - # return vels # TODO: Separate debug/viz class, bbox and 2d 3d points funcs def viz_object_states(self, cv_image, boxes, extracted_pts_all): @@ -239,6 +282,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.current_agent_obj_dims.clear() # Return if no track results available if track_result[0].boxes.id == None: return @@ -261,29 +306,27 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L # Separate numpy prob still faster for now obj_centers = self.find_centers(pedestrians_3d_pts) # Centers are calculated in lidar frame here obj_dims = self.find_dims(pedestrians_3d_pts) + # Pose is stored in vehicle frame and converted to start frame in the update function + obj_centers_vehicle = [] + for obj_center in obj_centers: + if len(obj_center) > 0: + obj_center = np.array([obj_center]) + obj_center_vehicle = transform_lidar_points(obj_center, self.R_lidar_to_vehicle, self.t_lidar_to_vehicle)[0] + obj_centers_vehicle.append(obj_center_vehicle) + else: + obj_centers_vehicle.append(np.array(())) + obj_centers = obj_centers_vehicle + + if(len(obj_center) != 0) and (len(obj_dims) != 0): + self.current_agent_obj_dims["pose"] = obj_center + self.current_agent_obj_dims["dims"] = obj_dims - # If in vehicle frame, transform centers from top_lidar frame to vehicle frame - # Need to transform the center point one by one since matrix op can't deal with empty points - if self.vehicle_frame: - obj_centers_vehicle = [] - for obj_center in obj_centers: - if len(obj_center) > 0: - obj_center = np.array([obj_center]) - obj_center_vehicle = transform_lidar_points(obj_center, self.R_lidar_to_vehicle, self.t_lidar_to_vehicle)[0] - obj_centers_vehicle.append(obj_center_vehicle) - else: - obj_centers_vehicle.append(np.array(())) - obj_centers = obj_centers_vehicle - - agents = self.create_agent_states(obj_centers=obj_centers, obj_dims=obj_dims) - - self.find_vels_and_ids(agents=agents) - # TODO: Refactor to make more efficient # TODO: Moving Average across last N iterations pos/vel? Less spurious vals - # TODO Fix velocity calculation to calculate in ObjectFrameEnum.START def find_vels_and_ids(self, agents: List[AgentState]): - self.current_agents.clear() + # self.prev_agents was assigned a deepcopy of self.current_agents and then self.current_agents + # was cleared before this function was called + # Nothing was scanned, erase current (for current output) and previous list (for next time through because nothing was scanned) if (len(agents) == 0): self.current_agents = {} @@ -315,7 +358,7 @@ def find_vels_and_ids(self, agents: List[AgentState]): agents[idx].activity = AgentActivityEnum.STOPPED if (np.all(vel == 0) or vel.size == 0) else AgentActivityEnum.MOVING agents[idx].velocity = (0, 0, 0) if vel.size == 0 else tuple(vel) - self.current_agents[prev_id] = copy.deepcopy(agents[idx]) + self.current_agents[prev_id] = agents[idx] del self.prev_agents[prev_id] # Remove previous agent from previous agents so it doesn't get assigned multiple times on accident break @@ -327,10 +370,9 @@ def find_vels_and_ids(self, agents: List[AgentState]): agents[idx].track_id = id agents[idx].activity = AgentActivityEnum.UNDETERMINED agents[idx].velocity = (0, 0, 0) - self.current_agents[id] = copy.deepcopy(agents[idx]) - self.prev_agents = copy.deepcopy(self.current_agents) + self.current_agents[id] = agents[idx] - # Calculates whether 2 agents overlap in START frame. True if they do, false if not + # Calculates whether 2 agents overlap. True if they do, false if not def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool: # Calculate corners of AgentState # Beware: AgentState(PhysicalObject) builds bbox from @@ -352,10 +394,12 @@ def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> List[AgentState]: # Gate to check whether dims and centers are empty (will happen if no pedestrians are scanned): for idx in range(len(obj_dims) -1, -1, -1): - if (obj_centers[idx].size == 0) or (obj_dims[idx].size == 0): + if (obj_centers[idx].size == 0) or (obj_dims[0].size == 0): + del obj_centers[idx] + del obj_dims[idx] + elif (obj_centers[idx].size != obj_dims[0].size): del obj_centers[idx] del obj_dims[idx] - # Create list of agent states in current vehicle frame: agents = [] num_pairings = len(obj_centers) @@ -378,18 +422,77 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n )) return agents + + + # TODO : Generate Transformation matrix from vehicle to start + # Just keeping this here incase we need it, at this moment this function will not be used. + # def generate_vehicle_start_frame(self) -> np.ndarray: + # ''' + # Creates T matrix for WORLD to START Frame and T matrix for WORLD to VEHICLE Frame + # Return T VEHICLE to START + # ''' + + # # T World to Start + # start_x, start_y, start_z = self.current_vehicle_state.pose.x, self.current_vehicle_state.pose.y, self.current_vehicle_state.pose.z + # start_yaw, start_roll, start_pitch = self.current_vehicle_state.pose.yaw, self.current_vehicle_state.pose.pitch, self.current_vehicle_state.pose.roll + # rotation_world_start = R.from_euler('xyz',[start_roll, start_pitch,start_yaw],degrees=False).as_matrix() + # self.t_start_to_world = np.eye(4) + # self.t_start_to_world[:3,:3] = rotation_world_start + # self.t_start_to_world[:3,3] = [start_x, start_y, start_z] + + + # # Converting vehicle_state from START to CURRENT + # self.current_vehicle_state.to_frame(frame=ObjectFrameEnum.CURRENT, current_pose=self.current_vehicle_state.pose, start_pose_abs=self.start_pose_abs) + + # # T World to Vehicle + # vehicle_x, vehicle_y, vehicle_z = self.current_vehicle_state.pose.x, self.current_vehicle_state.pose.y, self.current_vehicle_state.pose.z + # vehicle_yaw, vehicle_roll, vehicle_pitch = self.current_vehicle_state.pose.yaw, self.current_vehicle_state.pose.pitch, self.current_vehicle_state.pose.roll + # rotation_world_vehicle = R.from_euler('xyz',[vehicle_roll, vehicle_pitch, vehicle_yaw],degrees=False).as_matrix() + # self.t_vehicle_to_world = np.eye(4) + # self.t_vehicle_to_world[:3,:3] = rotation_world_vehicle + # self.t_vehicle_to_world[:3,3] = [vehicle_x, vehicle_y, vehicle_z] + + # return np.linalg.inv(self.t_start_to_world @ np.linalg.inv(self.t_vehicle_to_world)) + + + # TODO: Slower but cleaner to input self.current_agents dict + # TODO: Moving Average across last N iterations pos/vel? Less spurious vals + # TODO Akul: Fix velocity calculation to calculate in ObjectFrameEnum.START + # Work towards own tracking class instead of simple YOLO track? + # Fix 1: division by time + # Fix 2: Put centers and dims in start frame for velocity calc + final agentstate in update_object_states + # ret: Dict[track_id: vel[x, y, z]] + # def find_vels(self, track_ids: List[int], obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> Dict[int, np.ndarray]: + # # Object not seen -> velocity = None + # track_id_center_map = dict(zip(track_ids, obj_centers)) + # vels = defaultdict(lambda: np.array(())) # None is faster, np.array matches other find_ methods. + + # for prev_track_id, prev_agent in self.prev_agents.items(): + # if prev_track_id in track_ids: + # # TODO: Add prev_agents to memory to avoid None velocity + # # We should only be missing prev pose on first sight of track_id Agent. + # # print("shape 1: ", track_id_center_map[prev_agent.track_id]) + # # print("shape 2: ", np.array([prev_agent.pose.x, prev_agent.pose.y, prev_agent.pose.z])) + # # prev can be 3 separate Nones, current is just empty array... make this symmetrical + # if prev_agent.pose.x and prev_agent.pose.y and prev_agent.pose.z and track_id_center_map[prev_agent.track_id].shape == 3: + # vels[prev_track_id] = (track_id_center_map[prev_track_id] - np.array([prev_agent.pose.x, prev_agent.pose.y, prev_agent.pose.z])) / (self.curr_time - self.prev_time) + # return vels + + def ouster_oak_callback(self, cv_image: cv2.Mat, lidar_points: np.ndarray): - def ouster_oak_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2): # Update times for basic velocity calculation self.prev_time = self.curr_time self.curr_time = datetime.now() + self.cv_image = cv_image + self.lidar_points = lidar_points + # Convert to cv2 image and run detector - cv_image = self.bridge.imgmsg_to_cv2(rgb_image_msg, "bgr8") + # cv_image = self.bridge.imgmsg_to_cv2(rgb_image_msg, "bgr8") track_result = self.detector.track(source=cv_image, classes=self.classes_to_detect, persist=True, conf=self.confidence) # Convert 1D PointCloud2 data to x, y, z coords - lidar_points = convert_pointcloud2_to_xyz(lidar_pc2_msg) + # lidar_points = convert_pointcloud2_to_xyz(lidar_pc2_msg) # print("len lidar_points", len(lidar_points)) # Downsample xyz point clouds @@ -434,6 +537,7 @@ def ouster_oak_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2): extracted_pts_all.append(extracted_pts) else: extracted_pts_all.append(np.array(())) + # Generate Transformation matrix for vehicle to start self.update_object_states(track_result, extracted_pts_all) if self.debug: self.viz_object_states(cv_image, boxes, extracted_pts_all) diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index ef9a24851..958be7949 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -76,7 +76,7 @@ def update(self, state): return self.num_updates += 1 self.debug("vehicle","velocity",state.vehicle.v) - self.debug("vehicle","front wheel angle",state.vehicle.front_wheel_angle) + self.debug("vehicle","front wheel angle",state.vehicle.front_wheel_angle) 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/requirements.txt b/requirements.txt index 1679dba33..f8e808d90 100644 --- a/requirements.txt +++ b/requirements.txt @@ -10,4 +10,5 @@ dacite # Perception ultralytics -lap==0.5.12 \ No newline at end of file +lap==0.5.12 +open3d \ No newline at end of file