From 3c19b465cee69100621a243e917c9644a0126cf4 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:31:56 -0500 Subject: [PATCH 01/14] Better docstring --- GEMstack/utils/config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/utils/config.py b/GEMstack/utils/config.py index 9112107e9..ac7fbbaa7 100644 --- a/GEMstack/utils/config.py +++ b/GEMstack/utils/config.py @@ -18,7 +18,7 @@ def save_config(fn : str, config : dict) -> None: def load_config_recursive(fn : str) -> dict: - """Loads a configuration file with !include directives.""" + """Loads a configuration file with !include and !relative_path directives.""" if fn.endswith('yaml') or fn.endswith('yml'): with open(fn,'r') as f: res = yaml.load(f,_Loader) @@ -35,7 +35,7 @@ def load_config_recursive(fn : str) -> dict: class _Loader(yaml.SafeLoader): - """YAML Loader with `!include` constructor.""" + """YAML Loader with `!include` and `!relative_path` directives.""" def __init__(self, stream: IO) -> None: """Initialise Loader.""" @@ -60,7 +60,7 @@ def _construct_relative_path(loader: _Loader, node: yaml.Node) -> Any: yaml.add_constructor('!relative_path', _construct_relative_path, _Loader) def _load_config_or_text_recursive(fn : str) -> dict: - """Loads a configuration file with !include directives.""" + """Loads a configuration file with !include and !relative_path directives.""" if fn.endswith('yaml') or fn.endswith('yml'): with open(fn,'r') as f: res = yaml.load(f,_Loader) From 26eb65019b9c6a1f9a0fb844429edb1db0c74f07 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:56:54 -0500 Subject: [PATCH 02/14] Fixed to work with Klampt 0.10 --- .../visualization/klampt_visualization.py | 40 +++++++++++++------ 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/GEMstack/onboard/visualization/klampt_visualization.py b/GEMstack/onboard/visualization/klampt_visualization.py index 3e07aed23..762a747b4 100644 --- a/GEMstack/onboard/visualization/klampt_visualization.py +++ b/GEMstack/onboard/visualization/klampt_visualization.py @@ -1,4 +1,5 @@ from ..component import Component +from klampt import __version__ as klampt_version from klampt import vis from klampt.math import se3 from klampt import * @@ -11,14 +12,16 @@ import numpy as np class KlamptVisualization(Component): - """Runs a matplotlib visualization at 10Hz. - - If save_as is not None, saves the visualization to a file. + """Runs a Klampt visualization. + + Runs at 20Hz by default. """ def __init__(self, vehicle_interface, rate : float = 20.0, save_as : str = None): self.vehicle_interface = vehicle_interface self._rate = rate self.save_as = save_as + if save_as is not None: + print("WARNING: automatic saving of KlamptVisualization to movie is not supported yet. You can use Ctrl+M to start / stop saving the movie") self.num_updates = 0 self.last_yaw = None self.plot_values = {} @@ -57,12 +60,20 @@ def state_inputs(self): def initialize(self): vis.setWindowTitle("GEMstack visualization") vp = vis.getViewport() - vp.camera.rot[1] = -0.15 - vp.camera.rot[2] = -math.pi/2 - vp.camera.dist = 30.0 - vp.w = 1280 - vp.h = 720 - vp.clippingplanes = (0.1,1000) + if klampt_version == '0.10.0': + vp.controller.rot[1] = -0.15 + vp.controller.rot[2] = -math.pi/2 + vp.controller.dist = 30.0 + vp.resize(1280,720) + vp.n = 0.1 + vp.f = 1000 + else: + vp.camera.rot[1] = -0.15 + vp.camera.rot[2] = -math.pi/2 + vp.camera.dist = 30.0 + vp.w = 1280 + vp.h = 720 + vp.clippingplanes = (0.1,1000) vis.setViewport(vp) vis.add("vehicle_plane",self.world.terrain(0),hide_label=True) #note: show() takes over the interrupt handler and sets it to default, so we restore it @@ -121,9 +132,14 @@ def update(self, state): center_offset = 1.0 lookahead = 4.0*v dx,dy = math.cos(tracked_vehicle.pose.yaw)*(lookahead+center_offset),math.sin(tracked_vehicle.pose.yaw)*(lookahead+center_offset) - vp.camera.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5] - vp.camera.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw - vp.camera.dist += 5.0*(v - self.last_v) + if klampt_version == '0.10.0': + vp.controller.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5] + vp.controller.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw + vp.controller.dist += 5.0*(v - self.last_v) + else: + vp.camera.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5] + vp.camera.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw + vp.camera.dist += 5.0*(v - self.last_v) self.last_v = v vis.setViewport(vp) From 503a403e05e46eb1d09706918ce06c9a146b2bf6 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 17 Feb 2025 09:39:38 -0600 Subject: [PATCH 03/14] Fixed ROS type typo --- GEMstack/onboard/interface/gem_hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..e13ff817e 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -186,7 +186,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): From dc21e7e9a367bf0db7f66d1277bb87bdb8547a31 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 17 Feb 2025 09:39:38 -0600 Subject: [PATCH 04/14] Fixed ROS type typo --- GEMstack/onboard/interface/gem_hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..e13ff817e 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -186,7 +186,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): From 8031e3295eedcff795a2767a1d141f566ffa12d1 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 17 Feb 2025 22:22:08 -0500 Subject: [PATCH 05/14] Typo --- GEMstack/knowledge/calibration/gem_e4.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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" From 57811aae04511ab9bdb74b5a8492246bc1875f3a Mon Sep 17 00:00:00 2001 From: nvikramraj Date: Thu, 20 Feb 2025 00:27:38 -0600 Subject: [PATCH 06/14] integrated t vehicle to start --- GEMstack/onboard/interface/gem_hardware.py | 24 ++++++ .../perception/pedestrian_detection.py | 81 ++++++++++++++----- requirements.txt | 3 +- 3 files changed, 86 insertions(+), 22 deletions(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..b9fde1587 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -6,6 +6,7 @@ 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 +56,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 -------------------- @@ -227,6 +229,28 @@ 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': + 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, + 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'))) + topic1 = self.ros_sensor_topics['front_camera'] + topic2 = self.ros_sensor_topics['top_lidar'] + topic3 = self.ros_sensor_topics['gnss'] + self.front_camera_sub = message_filters.Subscriber(topic1, Image) + self.top_lidar_sub = message_filters.Subscriber(topic2, PointCloud2) + self.gnss_sub = message_filters.Subscriber(topic3, 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) # PACMod enable callback function diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index e145bcabf..7b672aef6 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -47,9 +47,10 @@ # 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 def box_to_fake_agent(box): """Creates a fake agent state from an (x,y,w,h) bounding box. @@ -97,12 +98,16 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: [-0.02124771, 0.99976695, -0.00381707], [-0.02289521, 0.00333035, 0.9997323]]) self.t_lidar_to_vehicle = np.array([[0.0], [0.0], [0.35]]) + self.t_start_to_world = None + self.t_vehicle_to_world = None + self.t_vehicle_to_start = None # 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) + # 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) + self.vehicle_interface.subscribe_sensor('sensor_fusion',self.ouster_oak_callback) # TF listener to get transformation from LiDAR to Camera self.tf_listener = tf.TransformListener() @@ -115,14 +120,8 @@ 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 and store it as GLOBAL + self.current_vehicle_state = VehicleState() def init_debug(self,) -> None: # Debug Publishers @@ -132,10 +131,6 @@ def init_debug(self,) -> None: self.pub_image = rospy.Publisher("/camera/image_detection", Image, queue_size=1) 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 return self.current_agents # TODO: Improve Algo Knn, ransac, etc. @@ -278,11 +273,41 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L obj_centers = obj_centers_vehicle self.find_vels_and_ids(obj_centers, obj_dims) - + + # TODO : Generate Transformation matrix from vehicle to start + 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: 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, obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]): + # 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[0].size == 0): @@ -440,17 +465,29 @@ def create_agent_states_in_start_frame(self, obj_centers: List[np.ndarray], obj_ # Return the agent states converted to start frame: return agents - def ouster_oak_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2): + def ouster_oak_callback(self, cv_image: cv2.Mat, lidar_points: np.ndarray, vehicle_state: GNSSReading): # Update times for basic velocity calculation self.prev_time = self.curr_time self.curr_time = datetime.now() + # Getting start pose in GLOBAL Frame + if self.start_pose_abs == None and vehicle_state.status == 'ok': + self.start_pose_abs = vehicle_state.pose + + # Update current vehicle state and convert to START Frame + if (vehicle_state.status == 'ok'): + self.current_vehicle_state.pose = vehicle_state.pose + self.current_vehicle_state.v = vehicle_state.speed + self.current_vehicle_state.to_frame(frame=ObjectFrameEnum.START, current_pose=self.current_vehicle_state.pose, start_pose_abs=self.start_pose_abs) + else: + raise Exception('Unable to get current pose of vehicle, lost GPS') + # 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 @@ -495,6 +532,8 @@ 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.t_vehicle_to_start = self.generate_vehicle_start_frame(vehicle_state) 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/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 From 7e0a49f96424b99d4302a6f886aae910565e78fb Mon Sep 17 00:00:00 2001 From: nvikramraj Date: Thu, 20 Feb 2025 00:52:19 -0600 Subject: [PATCH 07/14] syntax error fix --- GEMstack/onboard/perception/pedestrian_detection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index 7b672aef6..e75aa3412 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -533,7 +533,7 @@ def ouster_oak_callback(self, cv_image: cv2.Mat, lidar_points: np.ndarray, vehic else: extracted_pts_all.append(np.array(())) # Generate Transformation matrix for vehicle to start - self.t_vehicle_to_start = self.generate_vehicle_start_frame(vehicle_state) + self.t_vehicle_to_start = self.generate_vehicle_start_frame() self.update_object_states(track_result, extracted_pts_all) if self.debug: self.viz_object_states(cv_image, boxes, extracted_pts_all) From 57c54b9d9717126d5e650fe93e01dd4e09aba477 Mon Sep 17 00:00:00 2001 From: nvikramraj Date: Sat, 22 Feb 2025 12:10:41 -0600 Subject: [PATCH 08/14] code refactor for start_frame --- GEMstack/onboard/interface/gem_hardware.py | 25 ++- .../perception/pedestrian_detection.py | 191 +++++++++--------- 2 files changed, 119 insertions(+), 97 deletions(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index b9fde1587..042020f71 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -229,7 +229,7 @@ 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': + 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") @@ -243,14 +243,25 @@ def callback_with_cv2_numpy_gnss(rgb_image_msg: Image, lidar_pc2_msg: PointCloud ) 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'))) - topic1 = self.ros_sensor_topics['front_camera'] - topic2 = self.ros_sensor_topics['top_lidar'] - topic3 = self.ros_sensor_topics['gnss'] - self.front_camera_sub = message_filters.Subscriber(topic1, Image) - self.top_lidar_sub = message_filters.Subscriber(topic2, PointCloud2) - self.gnss_sub = message_filters.Subscriber(topic3, INSNavGeod) + 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 e75aa3412..43d0260fa 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -81,6 +81,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 @@ -98,16 +99,24 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: [-0.02124771, 0.99976695, -0.00381707], [-0.02289521, 0.00333035, 0.9997323]]) self.t_lidar_to_vehicle = np.array([[0.0], [0.0], [0.35]]) + + # 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 - # Subscribers and sychronizers + # 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) - self.vehicle_interface.subscribe_sensor('sensor_fusion',self.ouster_oak_callback) + + # GEMStack Subscribers and sychronizers + # LIDAR Camera fusion + self.vehicle_interface.subscribe_sensor('sensor_fusion_Lidar_Camera_GNSS',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() @@ -120,8 +129,8 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: # Update function variables self.t_start = datetime.now() # Estimated start frame time - self.start_pose_abs = None # Get this from GNSS and store it as GLOBAL - self.current_vehicle_state = VehicleState() + self.start_pose_abs = None # Get this from GNSS (GLOBAL frame) + self.current_vehicle_state = None # Current vehicle state from GNSS in GLOBAL frame def init_debug(self,) -> None: # Debug Publishers @@ -131,6 +140,23 @@ def init_debug(self,) -> None: self.pub_image = rospy.Publisher("/camera/image_detection", Image, queue_size=1) def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: + + # Convert to start state. + # self.current_vehicle_state | data from GNSS of vehicle pose in GLOBAL Frame + # self.start_pose_abs | data from GNSS of the first vehicle pose in GLOBAL Frame + vehicle.pose.to_frame(ObjectFrameEnum.START,self.current_vehicle_state.pose,self.start_pose_abs) + + # To be checked with Lucas + # self.prev_agents = self.current_agents.copy() + self.current_agents.clear() + + # Converting to start frame + obj_centers = vehicle.pose.apply(self.current_agent_obj_dims["pose"]) + obj_dims = vehicle.pose.apply(self.current_agent_obj_dims["dims"]) + + # Creating agent state and trackinig ids + self.find_vels_and_ids(obj_centers, obj_dims) + return self.current_agents # TODO: Improve Algo Knn, ransac, etc. @@ -149,29 +175,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): @@ -233,9 +236,9 @@ 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 if track_result[0].boxes.id == None: return @@ -259,49 +262,19 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L obj_centers = self.find_centers(pedestrians_3d_pts) # Centers are calculated in lidar frame here obj_dims = self.find_dims(pedestrians_3d_pts) - # 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 + # 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 - self.find_vels_and_ids(obj_centers, obj_dims) - - # TODO : Generate Transformation matrix from vehicle to start - 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)) + self.current_agent_obj_dims["pose"] = obj_center + self.current_agent_obj_dims["dims"] = obj_dims # TODO: Refactor to make more efficient # TODO: Moving Average across last N iterations pos/vel? Less spurious vals @@ -446,24 +419,65 @@ def create_agent_states_in_start_frame(self, obj_centers: List[np.ndarray], obj_ ) # Convert agent to start frame and add to agents list: - agents.append( - state - # state.to_frame( - # frame=ObjectFrameEnum.START, - # current_pose=self.current_vehicle_pose_sf, - # # current_pose=ObjectPose( - # # frame=ObjectFrameEnum.CURRENT, - # # t=(self.curr_time - self.t_start).total_seconds(), - # # x=state.pose.x, - # # y=state.pose.y, - # # z=state.pose.z - # # ), - # start_pose_abs=self.start_pose_abs - # ) - ) + agents.append(state) # Return the agent states converted to start frame: 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, vehicle_state: GNSSReading): # Update times for basic velocity calculation @@ -476,9 +490,7 @@ def ouster_oak_callback(self, cv_image: cv2.Mat, lidar_points: np.ndarray, vehic # Update current vehicle state and convert to START Frame if (vehicle_state.status == 'ok'): - self.current_vehicle_state.pose = vehicle_state.pose - self.current_vehicle_state.v = vehicle_state.speed - self.current_vehicle_state.to_frame(frame=ObjectFrameEnum.START, current_pose=self.current_vehicle_state.pose, start_pose_abs=self.start_pose_abs) + self.current_vehicle_state = vehicle_state else: raise Exception('Unable to get current pose of vehicle, lost GPS') @@ -533,7 +545,6 @@ def ouster_oak_callback(self, cv_image: cv2.Mat, lidar_points: np.ndarray, vehic else: extracted_pts_all.append(np.array(())) # Generate Transformation matrix for vehicle to start - self.t_vehicle_to_start = self.generate_vehicle_start_frame() self.update_object_states(track_result, extracted_pts_all) if self.debug: self.viz_object_states(cv_image, boxes, extracted_pts_all) From f2b17788951279950f835028319ffd09ae08e92b Mon Sep 17 00:00:00 2001 From: LucasEby Date: Sat, 22 Feb 2025 15:49:48 -0500 Subject: [PATCH 09/14] Updated velocity and id code to work off of data that's already in start frame --- .../perception/pedestrian_detection.py | 129 +++++++----------- 1 file changed, 46 insertions(+), 83 deletions(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index 43d0260fa..ce2f6ec91 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -140,23 +140,31 @@ def init_debug(self,) -> None: self.pub_image = rospy.Publisher("/camera/image_detection", Image, queue_size=1) def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: - # Convert to start state. # self.current_vehicle_state | data from GNSS of vehicle pose in GLOBAL Frame # self.start_pose_abs | data from GNSS of the first vehicle pose in GLOBAL Frame vehicle.pose.to_frame(ObjectFrameEnum.START,self.current_vehicle_state.pose,self.start_pose_abs) - # To be checked with Lucas - # self.prev_agents = self.current_agents.copy() - self.current_agents.clear() - # Converting to start frame obj_centers = vehicle.pose.apply(self.current_agent_obj_dims["pose"]) obj_dims = vehicle.pose.apply(self.current_agent_obj_dims["dims"]) - # Creating agent state and trackinig ids - self.find_vels_and_ids(obj_centers, obj_dims) - + # Prepare variables for find_vels_and_ids + self.prev_agents = self.current_agents.deepcopy() + 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=obj_centers, obj_dims=obj_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. @@ -236,8 +244,6 @@ 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: @@ -278,38 +284,25 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L # 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, obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]): - - # 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[0].size == 0): - del obj_centers[idx] - del obj_dims[idx] - + 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 + # Nothing was scanned, erase current (for current output) and previous list (for next time through because nothing was scanned) - if (len(obj_dims) == 0 or len(obj_centers) == 0): + if (len(agents) == 0): self.current_agents = {} - self.prev_agents = {} return - - new_prev_agents = {} # Stores current agents in START frame for next time through (since - # planning wants us to send them agents in CURRENT frame) assigned = False - num_pairings = len(obj_centers) + num_agents = len(agents) - # Create a list of agent states in the start frame - agents_sf = self.create_agent_states_in_start_frame(obj_centers=obj_centers, obj_dims=obj_dims) # Create agents in start frame - - # Loop through the indexes of the obj_center and obj_dim pairings - for idx in range(num_pairings): + for idx in range(num_agents): assigned = False # Loop through previous agents backwards for prev_id, prev_state in reversed(self.prev_agents.items()): - # If an obj_center and obj_dim pair overlaps with a previous agent, assume that they're the same agent - if self.agents_overlap(agents_sf[idx], prev_state): + # If a scanned agent overlaps with a previous agent, assume that they're the same agent + if self.agents_overlap(agents[idx], prev_state): assigned = True if self.prev_time == None: @@ -317,62 +310,30 @@ def find_vels_and_ids(self, obj_centers: List[np.ndarray], obj_dims: List[np.nda vel = np.array([0, 0, 0]) else: delta_t = self.curr_time - self.prev_time - vel = (np.array([agents_sf[idx].pose.x, agents_sf[idx].pose.y, agents_sf[idx].pose.z]) - np.array([prev_state.pose.x, prev_state.pose.y, prev_state.pose.z])) / delta_t.total_seconds() + vel = (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() print("VELOCITY:") print(vel) - - # Create current frame agent at planning group's request: - self.current_agents[prev_id] = ( - AgentState( - track_id = prev_id, - 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], yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), - # (l, w, h) - # TODO: confirm (z -> l, x -> w, y -> h) - dimensions=(obj_dims[idx][0], obj_dims[idx][1], obj_dims[idx][2]), - outline=None, - type=AgentEnum.PEDESTRIAN, - activity=AgentActivityEnum.STOPPED if np.all(vel == 0) else AgentActivityEnum.MOVING, - velocity=(0, 0, 0) if vel.size == 0 else tuple(vel), - yaw_rate=0 - )) # Fix start frame agent and store in this dict: - agents_sf[idx].track_id = prev_id - agents_sf[idx].activity = AgentActivityEnum.STOPPED if (np.all(vel == 0) or vel.size == 0) else AgentActivityEnum.MOVING - agents_sf[idx].velocity = (0, 0, 0) if vel.size == 0 else tuple(vel) + agents[idx].track_id = prev_id + 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) - new_prev_agents[prev_id] = agents_sf[idx] - del self.prev_agents[prev_id] # Remove previous agent from previous agents + 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 - - # If not assigned: + if not assigned: # Set velocity to 0 and assign the new agent a new id with IdTracker id = self.id_tracker.get_new_id() - # Create current frame agent at planning group's request: - self.current_agents[id] = ( - AgentState( - track_id = id, - 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] ,yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), - # (l, w, h) - # TODO: confirm (z -> l, x -> w, y -> h) - dimensions=(obj_dims[idx][0], obj_dims[idx][1], obj_dims[idx][2]), - outline=None, - type=AgentEnum.PEDESTRIAN, - activity=AgentActivityEnum.UNDETERMINED, - velocity=(0, 0, 0), - yaw_rate=0 - )) - # Fix start frame agent and store in this dict: - agents_sf[idx].track_id = id - agents_sf[idx].activity = AgentActivityEnum.UNDETERMINED - agents_sf[idx].velocity = (0, 0, 0) - new_prev_agents[id] = agents_sf[idx] - self.prev_agents = new_prev_agents + agents[idx].track_id = id + agents[idx].activity = AgentActivityEnum.UNDETERMINED + agents[idx].velocity = (0, 0, 0) + 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 @@ -396,13 +357,19 @@ def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool ( (z1_min <= z2_min and z2_min <= z1_max) or (z2_min <= z1_min and z1_min <= z2_max) ) ) - def create_agent_states_in_start_frame(self, obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> List[AgentState]: + 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[0].size == 0): + del obj_centers[idx] + del obj_dims[idx] + # Create list of agent states in current vehicle frame: agents = [] num_pairings = len(obj_centers) for idx in range(num_pairings): # Create agent in current frame: - state = AgentState( + agents.append(AgentState( track_id=0, # Temporary 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], yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), # Beware: AgentState(PhysicalObject) builds bbox from @@ -416,12 +383,8 @@ def create_agent_states_in_start_frame(self, obj_centers: List[np.ndarray], obj_ activity=AgentActivityEnum.UNDETERMINED, # Temporary velocity=None, # Temporary yaw_rate=0 - ) - - # Convert agent to start frame and add to agents list: - agents.append(state) + )) - # Return the agent states converted to start frame: return agents From 9601d8eb5935016fd47d2958d26194f94f1690ef Mon Sep 17 00:00:00 2001 From: nvikramraj Date: Mon, 24 Feb 2025 00:33:19 -0600 Subject: [PATCH 10/14] gnss integration --- GEMstack/knowledge/calibration/gem_e4.yaml | 2 +- GEMstack/onboard/interface/gem_hardware.py | 4 +- .../perception/pedestrian_detection.py | 71 +++++++++++++------ 3 files changed, 54 insertions(+), 23 deletions(-) diff --git a/GEMstack/knowledge/calibration/gem_e4.yaml b/GEMstack/knowledge/calibration/gem_e4.yaml index da971c253..241eacf80 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.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/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 042020f71..2de68d2a3 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -179,6 +179,7 @@ def callback_with_gnss_reading(inspva_msg: Inspva): else: def callback_with_gnss_reading(msg: INSNavGeod): pose = ObjectPose(ObjectFrameEnum.GLOBAL, + t = 0, x=msg.longitude, y=msg.latitude, z=msg.height, @@ -188,7 +189,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): @@ -234,6 +235,7 @@ def callback_with_cv2_numpy_gnss(rgb_image_msg: Image, lidar_pc2_msg: PointCloud 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, diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index ce2f6ec91..99796cada 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -51,6 +51,7 @@ 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. @@ -113,7 +114,7 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: # GEMStack Subscribers and sychronizers # LIDAR Camera fusion - self.vehicle_interface.subscribe_sensor('sensor_fusion_Lidar_Camera_GNSS',self.ouster_oak_callback) + 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) @@ -131,6 +132,7 @@ def __init__(self, vehicle_interface : GEMInterface) -> None: self.t_start = datetime.now() # 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 def init_debug(self,) -> None: # Debug Publishers @@ -139,22 +141,55 @@ 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]: - # Convert to start state. - # self.current_vehicle_state | data from GNSS of vehicle pose in GLOBAL Frame - # self.start_pose_abs | data from GNSS of the first vehicle pose in GLOBAL Frame - vehicle.pose.to_frame(ObjectFrameEnum.START,self.current_vehicle_state.pose,self.start_pose_abs) - # Converting to start frame - obj_centers = vehicle.pose.apply(self.current_agent_obj_dims["pose"]) - obj_dims = vehicle.pose.apply(self.current_agent_obj_dims["dims"]) + 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. + vehicle_start_pose = vehicle.pose.to_frame(ObjectFrameEnum.START,vehicle.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.deepcopy() + 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=obj_centers, obj_dims=obj_dims) + 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) @@ -244,7 +279,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.current_agent_obj_dims.clear() + # self.current_agent_obj_dims.clear() # Return if no track results available if track_result[0].boxes.id == None: return @@ -442,20 +477,14 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n # 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, vehicle_state: GNSSReading): + 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() - # Getting start pose in GLOBAL Frame - if self.start_pose_abs == None and vehicle_state.status == 'ok': - self.start_pose_abs = vehicle_state.pose - - # Update current vehicle state and convert to START Frame - if (vehicle_state.status == 'ok'): - self.current_vehicle_state = vehicle_state - else: - raise Exception('Unable to get current pose of vehicle, lost GPS') + 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") From ac1656363d08b8e478c812a3cbcfae29022c1004 Mon Sep 17 00:00:00 2001 From: nvikramraj Date: Mon, 24 Feb 2025 01:12:04 -0600 Subject: [PATCH 11/14] added viz and edge case fix --- GEMstack/onboard/perception/pedestrian_detection.py | 11 +++++++---- GEMstack/onboard/visualization/mpl_visualization.py | 8 ++++++++ launch/fixed_route.yaml | 1 + launch/pedestrian_detection.yaml | 7 ++++--- 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index 99796cada..36ad9c9dc 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -150,6 +150,8 @@ def gnss_test(self, cv_image: cv2.Mat, lidar_points: np.ndarray, vehicle_state: def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: + # 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 @@ -166,8 +168,8 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: print(f"Global state : {vehicle}") - # Convert pose to start state. - vehicle_start_pose = vehicle.pose.to_frame(ObjectFrameEnum.START,vehicle.pose,self.start_pose_abs) + # 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']}") @@ -314,8 +316,9 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L obj_centers_vehicle.append(np.array(())) obj_centers = obj_centers_vehicle - self.current_agent_obj_dims["pose"] = obj_center - self.current_agent_obj_dims["dims"] = obj_dims + 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 # TODO: Refactor to make more efficient # TODO: Moving Average across last N iterations pos/vel? Less spurious vals diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index ef9a24851..77a3e42cb 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. @@ -77,6 +78,13 @@ def update(self, state): 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..69f3b94b2 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -74,6 +74,7 @@ variants: 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..64a8e1c94 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -57,9 +57,10 @@ 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 c8495440032c50b109f82940e67948bf8c44e1ec Mon Sep 17 00:00:00 2001 From: nvikramraj Date: Mon, 24 Feb 2025 09:37:18 -0600 Subject: [PATCH 12/14] remove viz --- GEMstack/onboard/perception/pedestrian_detection.py | 3 +++ GEMstack/onboard/visualization/mpl_visualization.py | 10 +--------- launch/fixed_route.yaml | 1 - launch/pedestrian_detection.yaml | 7 +++---- 4 files changed, 7 insertions(+), 14 deletions(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index 36ad9c9dc..f0d7df4ee 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -401,6 +401,9 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n 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 = [] diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index 77a3e42cb..958be7949 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -5,7 +5,6 @@ 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. @@ -77,14 +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) - - # 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) - + 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/launch/fixed_route.yaml b/launch/fixed_route.yaml index 69f3b94b2..c05de8ff7 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -74,7 +74,6 @@ variants: 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 64a8e1c94..1a00fa797 100644 --- a/launch/pedestrian_detection.yaml +++ b/launch/pedestrian_detection.yaml @@ -57,10 +57,9 @@ variants: detector_only: run: description: "Run the pedestrian detection code" - drive: - planning: - trajectory_tracking: - visualization: !include "mpl_visualization.yaml" + drive: + planning: + trajectory_tracking: real_sim: run: description: "Run the pedestrian detection code with real detection and fake simulation" From 2b413fab941c7ab5ac6e6bc9a7dd37381f90874d Mon Sep 17 00:00:00 2001 From: krishauser Date: Mon, 24 Feb 2025 11:01:11 -0500 Subject: [PATCH 13/14] Added items committed to s2024 --- GEMstack/mathutils/dubins.py | 2 +- GEMstack/mathutils/transforms.py | 21 +++++++++++++++++++++ GEMstack/onboard/interface/gem_hardware.py | 9 ++++++--- 3 files changed, 28 insertions(+), 4 deletions(-) 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 e13ff817e..4fb95d4fc 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -1,6 +1,7 @@ from .gem import * from ...utils import settings import math +import time # ROS Headers import rospy @@ -151,6 +152,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 +171,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 +179,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=time.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), From 43bb3e8c53b6395fecb346d42b010fd052ab493d Mon Sep 17 00:00:00 2001 From: krishauser Date: Mon, 24 Feb 2025 11:04:02 -0500 Subject: [PATCH 14/14] Changed time.time to self.time --- GEMstack/onboard/interface/gem_hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 4fb95d4fc..836d7ef71 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -179,7 +179,7 @@ def callback_with_gnss_reading(inspva_msg: Inspva): else: def callback_with_gnss_reading(msg: INSNavGeod): pose = ObjectPose(ObjectFrameEnum.GLOBAL, - t=time.time(), + t=self.time(), x=math.degrees(msg.longitude), #Septentrio GNSS uses radians rather than degrees y=math.degrees(msg.latitude), z=msg.height,