there's some weird stuff going on with the left and right cameras... …

…I'm flummoxed

predicators/envs/assets/task_jsons/spot_bike_env/all_tools_vision/last.json

@@ -46,9 +46,9 @@
             "in_view": 1.0
         },
         "toolbag": {
-            "x": 7.43337,
-            "y": -8.40975,
-            "z": -0.3514145156906993
+            "x": 7.72705,
+            "y": -7.98225,
+            "z": -0.209524
         },
         "measuring_tape": {
             "x": 7.610710169137442,
@@ -60,10 +60,10 @@
         "spot": {
             "gripper_open_percentage": 0.9792149066925049,
             "curr_held_item_id": 0,
-            "x": 8.35358647550775,
-            "y": -6.962122638709362,
-            "z": 0.12450154693052401,
-            "yaw": 0.007795378752802229
+            "x": 7.2195,
+            "y": -7.61659,
+            "z": 0.109701,
+            "yaw": -1.51198
         }
     },
     "goal": {

predicators/envs/spot_env.py

@@ -994,13 +994,10 @@ def _reachable_classifier(cls, state: State,
         forward_unit = [np.cos(spot_pose[3]), np.sin(spot_pose[3])]
         spot_to_obj = np.subtract(obj_pose[:2], spot_pose[:2])
         spot_to_obj_unit = spot_to_obj / np.linalg.norm(spot_to_obj)
-        yaw_is_near = np.dot(forward_unit,
-                             spot_to_obj_unit) > cls._reachable_yaw_threshold
+        angle_between_robot_and_obj = np.arccos(np.dot(forward_unit, spot_to_obj_unit))
+        is_yaw_near = abs(angle_between_robot_and_obj) < cls._reachable_yaw_threshold
 
-        # if "toolbag" in obj.name:
-        #     import ipdb; ipdb.set_trace()
-
-        return is_xy_near and yaw_is_near
+        return is_xy_near and is_yaw_near
 
     @staticmethod
     def _surface_too_high_classifier(state: State,

predicators/spot_utils/perception_utils.py

@@ -418,16 +418,35 @@ def get_object_locations_with_detic_sam(
                                            }
 
     # Track the max scores found per object, assuming there is at most one.
-    obj_class_to_max_score: Dict[str, float] = {}
-
+    obj_class_to_max_score_and_source: Dict[str, Tuple[float, str]] = {}
     for source_name in CAMERA_NAMES:
         curr_res_segment = deticsam_results_all_cameras[source_name]
         for i, obj_class in enumerate(curr_res_segment['classes']):
             # Check that this particular class is one of the
             # classes we passed in, and that there was only one
             # instance of this class that was found.
-            assert obj_class.item() in classes
+            obj_cls_str = obj_class.item()
+            assert obj_cls_str in classes
             assert curr_res_segment['classes'].count(obj_class) == 1
+            score = curr_res_segment["scores"][i][0]
+            if obj_class_to_max_score_and_source.get(obj_cls_str) is None:
+                obj_class_to_max_score_and_source[obj_cls_str] = (score, source_name)
+            else:
+                if score > obj_class_to_max_score_and_source[obj_cls_str][0]:
+                    obj_class_to_max_score_and_source[obj_cls_str] = (score, source_name)
+
+    for source_name in CAMERA_NAMES:
+        curr_res_segment = deticsam_results_all_cameras[source_name]
+        for i, obj_class in enumerate(curr_res_segment['classes']):
+            # First, check if this is the highest scoring detection
+            # for this class amongst all detections from all sources.
+            score = curr_res_segment["scores"][i][0]
+            obj_cls_str = obj_class.item()
+            # Skip if we've already seen a higher-scoring detection
+            # for this object class from a different source.
+            if (score, source_name) != obj_class_to_max_score_and_source[obj_cls_str]:
+                continue
+
             # Compute median value of depth
             depth_median = np.median(
                 rotated_depth[curr_res_segment['masks'][i][0].squeeze()
@@ -469,6 +488,7 @@ def get_object_locations_with_detic_sam(
             inverse_rotation_angle = -ROTATION_ANGLE[source_name]
             fig = plt.figure()
             plt.imshow(depth_image_dict[source_name])
+            plt.colorbar()
             plt.imshow(ndimage.rotate(
                 curr_res_segment['masks'][i][0].squeeze(),
                 inverse_rotation_angle,
@@ -500,13 +520,12 @@ def get_object_locations_with_detic_sam(
                 depth_value=depth_median,
                 point_x=x_c_rotated,
                 point_y=y_c_rotated)
-            score = curr_res_segment["scores"][i][0]
-            obj_cls_str = obj_class.item()
-            # Skip if we've already seen a better object.
-            if obj_cls_str in obj_class_to_max_score and \
-                score < obj_class_to_max_score[obj_cls_str]:
-                continue
-            obj_class_to_max_score[obj_cls_str] = score
+
+            print(obj_cls_str)
+            print(depth_median)
+            print((x0, y0, z0))
+            # import ipdb; ipdb.set_trace()
+
             if not math.isnan(x0) and not math.isnan(y0) and not math.isnan(
                     z0):
                 ret_camera_to_obj_positions[source_name][obj_cls_str] = (x0,

predicators/spot_utils/spot_utils.py

@@ -724,7 +724,7 @@ def _scan_for_objects(
             waypoint = get_memorized_waypoint(waypoint_name)
             assert waypoint is not None
             waypoint_id, offset = waypoint
-            self.navigate_to(waypoint_id, offset)
+            # self.navigate_to(waypoint_id, offset)
             for _ in range(8):
                 objects_in_view: Dict[str, Tuple[float, float, float]] = {}
                 rgb_img_dict, rgb_img_response_dict, \
@@ -769,7 +769,7 @@ def _scan_for_objects(
                     break
                 logging.info("Still searching for objects:")
                 logging.info(remaining_objects)
-                self.relative_move(0.0, 0.0, np.pi / 4)
+                # self.relative_move(0.0, 0.0, np.pi / 4)
         return obj_poses
 
     def verify_estop(self, robot: Any) -> None: