diff --git a/riptide_mapping/config/dummy_detections.yaml b/riptide_mapping/config/dummy_detections.yaml index 77352d0..3499c00 100644 --- a/riptide_mapping/config/dummy_detections.yaml +++ b/riptide_mapping/config/dummy_detections.yaml @@ -21,7 +21,6 @@ # Bin - bin - - bin_target - bin_vinyl1 - bin_vinyl2 - bin_vinyl3 diff --git a/riptide_mapping/riptide_mapping2/dummydetections.py b/riptide_mapping/riptide_mapping2/dummydetections.py index faca4bb..275e78f 100644 --- a/riptide_mapping/riptide_mapping2/dummydetections.py +++ b/riptide_mapping/riptide_mapping2/dummydetections.py @@ -256,35 +256,37 @@ def timerCB(self): mapMat = resolvePoseInMap(parent) @ localMat mapPose = matToPose(mapMat) - - if publishInvalid: - #invalid quaternion indicating that mapping should not merge orientation - mapPose.orientation.w = 2.0 - mapPose.orientation.x = 2.0 - mapPose.orientation.y = 2.0 - mapPose.orientation.z = 2.0 - + # now convert pose to the desired camera frame fwd_camera_frame = self.get_parameter("forward_camera_frame").value.replace("", self.robot) #this is the frame that "detects" the object dwd_camera_frame = self.get_parameter("downward_camera_frame").value.replace("", self.robot) camera_frame = fwd_camera_frame if visibleForwards else dwd_camera_frame if visibleDownwards else None - + map2camera = self.tfBuffer.lookup_transform(camera_frame, "map", rclpy.time.Time()) framePose = do_transform_pose(mapPose, map2camera) - + + if publishInvalid: + #invalid quaternion indicating that mapping should not merge orientation. + #set after the frame transform so the sentinel reaches mapping unchanged. + framePose.orientation.w = 2.0 + framePose.orientation.x = 2.0 + framePose.orientation.y = 2.0 + framePose.orientation.z = 2.0 + pubPose = PoseWithCovarianceStamped() pubPose.header = fwdHeader if visibleForwards else dwdHeader if visibleDownwards else None pubPose.pose.pose = framePose self.pubs[i].publish(pubPose) - - #rotate quaternion to the "z out" position - transformedQuat = [framePose.orientation.w, framePose.orientation.x, framePose.orientation.y, framePose.orientation.z] - rotatedQuat = tf3d.quaternions.qmult(transformedQuat, CAMERA_ROTATION) #wxyz - - framePose.orientation.w = rotatedQuat[0] - framePose.orientation.x = rotatedQuat[1] - framePose.orientation.y = rotatedQuat[2] - framePose.orientation.z = rotatedQuat[3] + + #rotate quaternion to the "z out" position (skip for the invalid sentinel, which must stay exact) + if not publishInvalid: + transformedQuat = [framePose.orientation.w, framePose.orientation.x, framePose.orientation.y, framePose.orientation.z] + rotatedQuat = tf3d.quaternions.qmult(transformedQuat, CAMERA_ROTATION) #wxyz + + framePose.orientation.w = rotatedQuat[0] + framePose.orientation.x = rotatedQuat[1] + framePose.orientation.y = rotatedQuat[2] + framePose.orientation.z = rotatedQuat[3] #populate detection. looks like mapping only uses results so I'll just populate that and also header because its easy detection = Detection3D() diff --git a/riptide_mapping/riptide_mapping2/mapping.py b/riptide_mapping/riptide_mapping2/mapping.py index 71b79a6..01038fb 100755 --- a/riptide_mapping/riptide_mapping2/mapping.py +++ b/riptide_mapping/riptide_mapping2/mapping.py @@ -182,7 +182,7 @@ def __init__(self): self.instance2_seeded = False self.add_on_set_parameters_callback(self.param_callback) - self.create_subscription(Detection3DArray, "detected_objects".format(self.get_namespace()), self.vision_callback, qos_profile_system_default) + self.create_subscription(Detection3DArray, "detected_objects", self.vision_callback, qos_profile_system_default) self.status_pub = self.create_publisher(MappingTargetInfo, "state/mapping", qos_profile_system_default) self.create_service(MappingTarget, "mapping_target", self.target_callback) # Should prob be mapping ns but not changing for compatability for now self.create_service(Trigger, "mapping/reset_mapping", self.reset_mapping_callback) diff --git a/tensor_detector/src/detection.py b/tensor_detector/src/detection.py index 9588ac7..a43e1fa 100644 --- a/tensor_detector/src/detection.py +++ b/tensor_detector/src/detection.py @@ -285,7 +285,7 @@ def create_detection3d_message(self, box, frame, conf): # Slalom: depth-sampled centroid, default-facing orientation (mapping overrides it now) if class_name == SLALOM_CLASS: depth_value = frame.depth[int(bbox_center_y), int(bbox_center_x)] - if (np.isnan(depth_value) or math.isinf(bbox_center_x) + if (np.isnan(depth_value) or depth_value == 0 or math.isinf(bbox_center_x) or math.isinf(bbox_center_y) or math.isinf(depth_value)): return None centroid = geometry.pixel_to_3d(bbox_center_x, bbox_center_y, @@ -571,7 +571,6 @@ def _emit_combined_pair(self, frame, detections, boxes_a, boxes_b, pair_name): if table_quat is not None: quat = table_quat - self.log.info("using table pair quat") bbox_width = union[2] - union[0] bbox_height = union[3] - union[1] @@ -581,10 +580,13 @@ def _emit_combined_pair(self, frame, detections, boxes_a, boxes_b, pair_name): detections.detections.append(detection) def _table_pair_quat(self, frame, fit, warning_boxes, helmet_boxes): - """Table orientation, flat in the world frame: +z straight up, +x horizontal - and perpendicular to the warning->helmet line (toward a free edge of the - square table). Returned in the camera frame; None (caller keeps the - plane-fit quat) if the world tf is unavailable.""" + """Table orientation parallel to the world floor. + + Local +z follows world +z, while local +x points toward a basket-free + edge, perpendicular to the horizontal warning->helmet line. Returned + in the camera frame; None (caller keeps the plane-fit quat) if the + world tf is unavailable or the basket heading is degenerate. + """ try: tf_q = self.tf_buffer.lookup_transform( frame.frame_id, WORLD_FRAME, Time()).transform.rotation @@ -605,8 +607,13 @@ def _table_pair_quat(self, frame, fit, warning_boxes, helmet_boxes): wh_world = geometry.quat_rotate(cam_from_world, member_pts[1] - member_pts[0], inverse=True) - across_world = np.cross([0.0, 1.0, 0.0], wh_world) # horizontal, perp to W->H - table_in_world = geometry.quat_from_normal_and_inplane_dir([0.0, 1.0, 0.0], + world_up = np.array([0.0, 0.0, 1.0]) + wh_world[2] = 0.0 + if np.linalg.norm(wh_world) < 1e-8: + return None + + across_world = np.cross(world_up, wh_world) + table_in_world = geometry.quat_from_normal_and_inplane_dir(world_up, across_world) return quaternion_multiply(cam_from_world, table_in_world) diff --git a/tensor_detector/src/yolo_orientation.py b/tensor_detector/src/yolo_orientation.py index ead3af4..009e143 100644 --- a/tensor_detector/src/yolo_orientation.py +++ b/tensor_detector/src/yolo_orientation.py @@ -102,6 +102,10 @@ def __init__(self): self._depth_lock = threading.Lock() self._depth_buffer = deque(maxlen=self.depth_buffer_size) + # Guards the camera/model/config fields setup_camera() swaps on a camera + # switch, so image_callback never reads a torn mix of old/new values. + self._camera_lock = threading.Lock() + # tf self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) @@ -214,47 +218,60 @@ def delayed_setup(self): ### Cammera lifecycle def setup_camera(self): self.get_logger().info(f"Active camera: {self.active_camera}") - self.camera_prefix = self.active_camera - self.frame_id = f'{self.robot_ns}/{self.camera_prefix}_left_camera_optical_frame' yolo_model = self.get_parameter(f'{self.active_camera}_model').get_parameter_value().string_value class_id_map_str = self.get_parameter(f'{self.active_camera}_class_id_map').get_parameter_value().string_value - self.conf = self.get_parameter(f'{self.active_camera}_threshold').get_parameter_value().double_value - self.iou = self.get_parameter(f'{self.active_camera}_iou').get_parameter_value().double_value + conf = self.get_parameter(f'{self.active_camera}_threshold').get_parameter_value().double_value + iou = self.get_parameter(f'{self.active_camera}_iou').get_parameter_value().double_value self.get_logger().info(f"Yolo Model: {yolo_model}") self.get_logger().info(f"Class id map str: {class_id_map_str}") - self.get_logger().info(f"Confidence Threshold: {self.conf}") - self.get_logger().info(f"IOU: {self.iou}") - - self.load_class_id_map(class_id_map_str) + self.get_logger().info(f"Confidence Threshold: {conf}") + self.get_logger().info(f"IOU: {iou}") - # Task profile: torpedo (fire/blood) logic runs only on the chosen camera (ffc really, but generic here cause why not) - # On the other camera those classes fall through to the standard path (so bins works like normal) - self.detector.set_torpedo_enabled(self.active_camera == self.torpedo_task_camera) + class_id_map = self.parse_class_id_map(class_id_map_str) weights_dir = os.path.join(get_package_share_directory("tensor_detector"), 'weights') model_path = os.path.join(weights_dir, yolo_model) self.get_logger().info(f"Loading model path: {model_path}") - self.model = YoloModel(model_path=model_path, export=self.export, logger=self.get_logger()) + model = YoloModel(model_path=model_path, export=self.export, logger=self.get_logger()) + + camera_prefix = self.active_camera + frame_id = f'{self.robot_ns}/{camera_prefix}_left_camera_optical_frame' + + # Swap all camera/model state together so image_callback never reads a + # mix of old and new values (e.g. new model with old conf/iou). + with self._camera_lock: + self.camera_prefix = camera_prefix + self.frame_id = frame_id + self.conf = conf + self.iou = iou + self.class_id_map = class_id_map + self.model = model + # Task profile: torpedo (fire/blood) logic runs only on the chosen camera (ffc really, but generic here cause why not) + # On the other camera those classes fall through to the standard path (so bins works like normal) + self.detector.set_torpedo_enabled(self.active_camera == self.torpedo_task_camera) self.reset_collection_variables() self.destroy_subscriptions() self.create_subscriptions() - def load_class_id_map(self, class_id_map_str): + def parse_class_id_map(self, class_id_map_str): # YAML is the single source of truth for the class map - self.class_id_map = yaml.safe_load(class_id_map_str) if class_id_map_str else {} - if not self.class_id_map: + class_id_map = yaml.safe_load(class_id_map_str) if class_id_map_str else {} + if not class_id_map: + class_id_map = {} # Not crashing out here because there's if the other camera config is empty after switch the node would crash self.get_logger().warning("No class_id_map provided in params; no detections will be produced.") - self.get_logger().info(f"Class id map: {self.class_id_map}") + self.get_logger().info(f"Class id map: {class_id_map}") + return class_id_map def reset_collection_variables(self): with self._depth_lock: self._depth_buffer.clear() - self.camera_info_gathered = False + with self._camera_lock: + self.camera_info_gathered = False def destroy_subscriptions(self): for attr in ('zed_info_subscription', 'image_subscription', 'depth_subscription'): @@ -293,12 +310,13 @@ def camera_info_callback(self, msg): if not self.camera_info_gathered: if self.print_camera_info: self.get_logger().info(f"Camera info: {msg}") - self.intrinsic_matrix = np.array(msg.k).reshape((3, 3)) - self.fx = msg.k[0] - self.cx = msg.k[2] - self.fy = msg.k[4] - self.cy = msg.k[5] - self.camera_info_gathered = True + with self._camera_lock: + self.intrinsic_matrix = np.array(msg.k).reshape((3, 3)) + self.fx = msg.k[0] + self.cx = msg.k[2] + self.fy = msg.k[4] + self.cy = msg.k[5] + self.camera_info_gathered = True def depth_callback(self, msg): now = self.get_clock().now() @@ -355,19 +373,30 @@ def image_callback(self, msg: CompressedImage): if cv_image is None: return + # Snapshot the camera/model state together so a concurrent setup_camera() + # (camera switch) can't hand us a mix of old and new values mid-frame. + with self._camera_lock: + model = self.model + conf = self.conf + iou = self.iou + class_id_map = self.class_id_map + frame_id = self.frame_id + fx, fy, cx, cy = self.fx, self.fy, self.cx, self.cy + intrinsic_matrix = self.intrinsic_matrix + # Run inference - results = self.model.infer(cv_image, conf=self.conf, iou=self.iou) + results = model.infer(cv_image, conf=conf, iou=iou) # Store everything we need in the frame dataclass 🤌 frame = Frame( image=cv_image, depth=depth_image, - fx=self.fx, fy=self.fy, cx=self.cx, cy=self.cy, - K=self.intrinsic_matrix, - frame_id=self.frame_id, + fx=fx, fy=fy, cx=cx, cy=cy, + K=intrinsic_matrix, + frame_id=frame_id, timestamp=self._stamp(msg), - class_id_map=self.class_id_map, - conf=self.conf, + class_id_map=class_id_map, + conf=conf, want_markers=self.has_subscribers(self.marker_array_publisher), want_cloud=self.has_subscribers(self.point_cloud_publisher), want_overlay_points=self.has_subscribers(self.annotated_image_publisher) @@ -388,7 +417,7 @@ def image_callback(self, msg: CompressedImage): # Publish point cloud for visualization if self.has_subscribers(self.point_cloud_publisher): - cloud = self.detector.take_point_cloud(self.frame_id, self._stamp(msg)) + cloud = self.detector.take_point_cloud(frame_id, self._stamp(msg)) if cloud is not None: self.point_cloud_publisher.publish(cloud)