Post comp cleanup

riptide_mapping/config/dummy_detections.yaml

@@ -21,7 +21,6 @@
 
       # Bin
       - bin
-      - bin_target
       - bin_vinyl1
       - bin_vinyl2
       - bin_vinyl3

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("<robot>", self.robot) #this is the frame that "detects" the object
                     dwd_camera_frame = self.get_parameter("downward_camera_frame").value.replace("<robot>", 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()

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)

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)
 

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)