update

src/vlm_nav_pkg/config/example_object_detection_vlm.json

@@ -1,47 +1,47 @@
 {
     "Refrigerator": {
-        "highest_score": 0.301025390625,
+        "highest_score": 0.301513671875,
-        "last_detected": "2026-04-29T11:38:32.954873",
+        "last_detected": "2026-05-18T14:13:25.085735",
         "position": {
-            "x": -3.258748492688493,
+            "x": -1.3181,
-            "y": -0.657160436573238,
+            "y": 0.5165,
-            "z": 0.23105746596100607
+            "z": 0.7319
         }
     },
     "water dispenser": {
-        "highest_score": 0.310791015625,
+        "highest_score": 0.316162109375,
-        "last_detected": "2026-04-29T11:38:29.064743",
+        "last_detected": "2026-05-18T14:15:56.440127",
         "position": {
-            "x": -2.7678260060491566,
+            "x": -1.6292,
-            "y": -0.2820617355615878,
+            "y": -1.7906,
-            "z": -0.006539989127681528
+            "z": 0.661
         }
     },
     "sofa": {
-        "highest_score": 0.316650390625,
+        "highest_score": 0.307861328125,
-        "last_detected": "2026-04-29T11:36:16.740557",
+        "last_detected": "2026-05-18T14:17:27.585956",
         "position": {
-            "x": -1.444039469788105,
+            "x": 1.4535,
-            "y": -1.389487320024688,
+            "y": -0.4709,
-            "z": 0.9400984640933344
+            "z": 0.5445
         }
     },
     "white toilet": {
-        "highest_score": 0.29931640625,
+        "highest_score": 0.269287109375,
-        "last_detected": "2026-04-29T11:32:23.241802",
+        "last_detected": "2026-05-18T14:13:07.308074",
         "position": {
-            "x": 0.5281400126286391,
+            "x": -4.3598,
-            "y": 7.222577558743074,
+            "y": -1.1593,
-            "z": 0.24172188472363199
+            "z": 0.3363
         }
     },
     "office chair with wheels": {
-        "highest_score": 0.316650390625,
+        "highest_score": 0.3037109375,
-        "last_detected": "2026-04-29T11:34:58.447898",
+        "last_detected": "2026-05-18T14:12:39.416197",
         "position": {
-            "x": 3.982877977168086,
+            "x": -2.8461,
-            "y": 1.6895332207339573,
+            "y": 2.6732,
-            "z": 0.5514825779390112
+            "z": 0.4811
         }
     }
 }

src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py

@@ -1,24 +1,41 @@
 #!/usr/bin/env python3
 import json
+import math
+import time
 import clip
 import torch
+from copy import deepcopy
 import os
-from math import isfinite
-import yaml
-import cv2
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import PoseStamped
+from tf2_ros import Buffer, TransformListener
+from rclpy.duration import Duration
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
+from nav_msgs.msg import OccupancyGrid
 from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 from ament_index_python.packages import get_package_share_directory
+
+
 class SemanticNavNode(Node):
     def __init__(self, json_path: str):
         super().__init__('semantic_nav_node')
+        self.declare_parameter('approach_offset_m', 0.7)
+        self.declare_parameter('approach_offset_refrigerator_m', 0.7)
+        self.declare_parameter('approach_offset_water_dispenser_m', 0.7)
+        self.declare_parameter('approach_offset_sofa_m', 0.5)
+        self.declare_parameter('approach_offset_white_toilet_m', 0.7)
+        self.declare_parameter('approach_offset_office_chair_with_wheels_m', 0.45)
+        self.declare_parameter('approach_candidate_count', 8)
+        self.declare_parameter('approach_clearance_m', 0.30)
+        self.declare_parameter('approach_clearance_office_chair_with_wheels_m', 0.18)
+        self.declare_parameter('approach_candidate_count_office_chair_with_wheels', 16)
+        self.declare_parameter('approach_offset_office_chair_with_wheels_search_radii', [0.45, 0.60, 0.75])
+        self.map_msg = None
 
         # Load object library
         with open(json_path) as f:
             self.object_lib = json.load(f)
-        self.map_bounds = self._load_map_bounds()
 
         # Load CLIP
         self.device = "cuda" if torch.cuda.is_available() else "cpu"
@@ -34,73 +51,21 @@ class SemanticNavNode(Node):
         # Initialize navigator
         self.navigator = BasicNavigator()
         self.navigator.waitUntilNav2Active()
-        self.get_logger().info("Navigator ready")
+        self.tf_buffer = Buffer(cache_time=Duration(seconds=10))
-
+        self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True)
-    def _load_map_bounds(self):
+        map_qos = QoSProfile(
-        map_yaml = os.environ.get('NAV2_MAP_PATH', '').strip()
+            depth=1,
-        map_yaml = os.path.expanduser(map_yaml) if map_yaml else ''
+            reliability=ReliabilityPolicy.RELIABLE,
-        if not map_yaml or not os.path.isfile(map_yaml):
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
-            try:
+            history=HistoryPolicy.KEEP_LAST,
-                tb3_dir = get_package_share_directory('turtlebot3_gazebo')
-                map_yaml = os.path.join(tb3_dir, 'map', 'office_map.yaml')
-            except Exception:
-                map_yaml = ''
-        if not map_yaml or not os.path.isfile(map_yaml):
-            self.get_logger().warn(
-                "Map bounds unavailable in semantic_nav; "
-                "fallback selection will skip bounds checks."
-            )
-            return None
-        try:
-            with open(map_yaml, 'r') as f:
-                cfg = yaml.safe_load(f)
-            resolution = float(cfg['resolution'])
-            ox, oy = float(cfg['origin'][0]), float(cfg['origin'][1])
-            image_path = str(cfg['image'])
-            if not os.path.isabs(image_path):
-                image_path = os.path.join(os.path.dirname(map_yaml), image_path)
-            img = cv2.imread(image_path, cv2.IMREAD_UNCHANGED)
-            if img is None:
-                raise RuntimeError(f"cannot open map image: {image_path}")
-            h, w = img.shape[:2]
-            bounds = {
-                "min_x": ox,
-                "max_x": ox + w * resolution,
-                "min_y": oy,
-                "max_y": oy + h * resolution,
-            }
-            self.get_logger().info(
-                "Semantic map bounds: "
-                f"x=[{bounds['min_x']:.3f}, {bounds['max_x']:.3f}], "
-                f"y=[{bounds['min_y']:.3f}, {bounds['max_y']:.3f}]"
-            )
-            return bounds
-        except Exception as e:
-            self.get_logger().warn(f"Failed to load map bounds: {e}")
-            return None
-
-    def _sanitize_position(self, pos):
-        if not isinstance(pos, dict):
-            return None
-        try:
-            x = float(pos["x"])
-            y = float(pos["y"])
-            z = float(pos.get("z", 0.0))
-        except Exception:
-            return None
-        if not (isfinite(x) and isfinite(y) and isfinite(z)):
-            return None
-        return {"x": x, "y": y, "z": z}
-
-    def _in_map_bounds(self, pos):
-        if pos is None:
-            return False
-        if self.map_bounds is None:
-            return True
-        return (
-            self.map_bounds["min_x"] <= pos["x"] <= self.map_bounds["max_x"]
-            and self.map_bounds["min_y"] <= pos["y"] <= self.map_bounds["max_y"]
         )
+        self.map_sub = self.create_subscription(
+            OccupancyGrid,
+            "/map",
+            self.map_callback,
+            map_qos,
+        )
+        self.get_logger().info("Navigator ready")
 
     def query_object(self, prompt: str):
         tokens = clip.tokenize([prompt]).to(self.device)
@@ -112,46 +77,18 @@ class SemanticNavNode(Node):
         best_idx = sims.argmax().item()
         best_name = self.object_names[best_idx]
 
-        # Align with vlm_detection:
+        # Align with vlm_detection: position may be None if not yet detected
-        # prefer best_position/position, fallback to last_position when best is out-of-bounds.
         obj_info = self.object_lib.get(best_name, {})
-        best_pos_raw = obj_info.get("best_position")
+        pos = obj_info.get("position")
-        if best_pos_raw is None:
-            best_pos_raw = obj_info.get("position")
-        last_pos_raw = obj_info.get("last_position")
-        best_pos = self._sanitize_position(best_pos_raw)
-        last_pos = self._sanitize_position(last_pos_raw)
 
-        if best_pos is not None and self._in_map_bounds(best_pos):
+        if pos is None:
-            return best_name, best_pos
-
-        if best_pos is not None and not self._in_map_bounds(best_pos):
             self.get_logger().warn(
-                f"'{best_name}' best_position out of map bounds: {best_pos}"
+                f"'{best_name}' matched but has no position in JSON. "
-            )
-            if last_pos is not None and self._in_map_bounds(last_pos):
-                self.get_logger().warn(
-                    f"Falling back to last_position for '{best_name}': {last_pos}"
-                )
-                return best_name, last_pos
-
-        if last_pos is not None and self._in_map_bounds(last_pos):
-            self.get_logger().warn(
-                f"Using last_position for '{best_name}': {last_pos}"
-            )
-            return best_name, last_pos
-
-        if best_pos is None and last_pos is None:
-            self.get_logger().warn(
-                f"'{best_name}' matched but has no usable position in JSON. "
                 f"Run vlm_detection in AMCL mode first!"
             )
             return best_name, None
 
-        self.get_logger().warn(
+        return best_name, pos
-            f"'{best_name}' has only out-of-bounds position(s), cannot navigate."
-        )
-        return best_name, None
 
     def navigate_to_object(self, prompt: str):
         name, pos = self.query_object(prompt)
@@ -159,35 +96,299 @@ class SemanticNavNode(Node):
             self.get_logger().warn(f"Cannot navigate: no valid position for '{name}'. Please run vlm_detection first!")
             return
 
+        robot_pose = self.get_robot_pose_in_map()
+        if robot_pose is None:
+            self.get_logger().warn("Cannot navigate: failed to get robot pose in map frame.")
+            return
+
+        offset_m = self.get_approach_offset(name)
+        goal_x, goal_y, yaw = self.compute_approach_goal(name, robot_pose, pos, offset_m)
+        robot_x, robot_y = robot_pose
+        distance_to_goal = math.hypot(goal_x - robot_x, goal_y - robot_y)
+
         goal_pose = PoseStamped()
         goal_pose.header.frame_id = "map"
         goal_pose.header.stamp = self.navigator.get_clock().now().to_msg()
-        goal_pose.pose.position.x = pos["x"]
+        goal_pose.pose.position.x = goal_x
-        goal_pose.pose.position.y = pos["y"]
+        goal_pose.pose.position.y = goal_y
-        goal_pose.pose.position.z = pos.get("z", 0.0)
+        goal_pose.pose.position.z = 0.0
-        goal_pose.pose.orientation.z = 0.0
+        goal_pose.pose.orientation.z = math.sin(yaw / 2.0)
-        goal_pose.pose.orientation.w = 1.0
+        goal_pose.pose.orientation.w = math.cos(yaw / 2.0)
 
-        self.navigator.followWaypoints([goal_pose])
+        self.navigator.goToPose(deepcopy(goal_pose))
-        self.get_logger().info(f"Navigating to {name} at {pos}")
+        self.get_logger().info(
+            f"Navigating to {name}: object={pos}, goal={{'x': {goal_x:.4f}, 'y': {goal_y:.4f}}}, "
+            f"approach_offset={offset_m:.2f}m"
+        )
+        self.get_logger().info(
+            f"Robot pose={{'x': {robot_x:.4f}, 'y': {robot_y:.4f}}}, "
+            f"distance_to_goal={distance_to_goal:.4f}m"
+        )
 
         # Wait until navigation completes
         while not self.navigator.isTaskComplete():
             feedback = self.navigator.getFeedback()
             if feedback:
                 self.get_logger().info(
-                    f"Executing waypoint {feedback.current_waypoint + 1}/1"
+                    "Navigation task in progress"
                 )
 
         result = self.navigator.getResult()
+        final_robot_pose = self.get_robot_pose_in_map()
+        if final_robot_pose is not None:
+            final_x, final_y = final_robot_pose
+            final_distance_to_goal = math.hypot(goal_x - final_x, goal_y - final_y)
+            self.get_logger().info(
+                f"Final robot map pose={{'x': {final_x:.4f}, 'y': {final_y:.4f}}}, "
+                f"distance_to_goal={final_distance_to_goal:.4f}m"
+            )
         if result == TaskResult.SUCCEEDED:
             self.get_logger().info("Navigation succeeded")
         elif result == TaskResult.CANCELED:
             self.get_logger().info("Navigation canceled")
         elif result == TaskResult.FAILED:
             self.get_logger().info("Navigation failed")
-            
+
-            
+    def map_callback(self, msg: OccupancyGrid):
+        self.map_msg = msg
+
+    def get_robot_pose_in_map(self):
+        target_frame = "map"
+        source_frame = "base_footprint"
+        last_error = None
+        for _ in range(15):
+            try:
+                if not self.tf_buffer.can_transform(
+                    target_frame,
+                    source_frame,
+                    rclpy.time.Time(),
+                    timeout=Duration(seconds=0.2)
+                ):
+                    continue
+
+                transform = self.tf_buffer.lookup_transform(
+                    target_frame,
+                    source_frame,
+                    rclpy.time.Time(),
+                    timeout=Duration(seconds=0.2)
+                )
+                return (
+                    transform.transform.translation.x,
+                    transform.transform.translation.y,
+                )
+            except Exception as e:
+                last_error = e
+
+        if last_error is None:
+            self.get_logger().warn(
+                "Robot pose lookup failed: transform map -> base_footprint was not available within timeout."
+            )
+        else:
+            self.get_logger().warn(f"Robot pose lookup failed: {last_error}")
+        return None
+
+    def compute_approach_goal(self, object_name, robot_pose, obj_pos, offset_m):
+        robot_x, robot_y = robot_pose
+        obj_x = obj_pos["x"]
+        obj_y = obj_pos["y"]
+        search_radii = self.get_approach_search_radii(object_name, offset_m)
+        candidate_count = self.get_approach_candidate_count(object_name)
+        approach_candidates = []
+        for radius in search_radii:
+            approach_candidates.extend(
+                self.generate_approach_candidates(obj_x, obj_y, radius, candidate_count)
+            )
+        feasible_candidates = [
+            candidate for candidate in approach_candidates
+            if self.is_candidate_feasible(object_name, candidate[0], candidate[1])
+        ]
+
+        path_candidates = []
+        start_pose = self.build_pose_stamped(robot_x, robot_y, 0.0)
+        for candidate in feasible_candidates:
+            candidate_x, candidate_y, candidate_yaw = candidate
+            candidate_goal_pose = self.build_pose_stamped(candidate_x, candidate_y, candidate_yaw)
+            path = self.navigator.getPath(start_pose, candidate_goal_pose, use_start=True)
+            if path and len(path.poses) >= 2:
+                path_length = self.compute_path_length(path)
+                if path_length >= 0.2:
+                    path_candidates.append((candidate_x, candidate_y, candidate_yaw, path_length))
+
+        if path_candidates:
+            goal_x, goal_y, yaw, path_length = min(
+                path_candidates,
+                key=lambda candidate: candidate[3]
+            )
+            self.get_logger().info(
+                f"Approach candidates for {object_name}: total={len(approach_candidates)} "
+                f"feasible={len(feasible_candidates)} path_feasible={len(path_candidates)} "
+                f"selected=({goal_x:.4f}, {goal_y:.4f}) path_length={path_length:.4f}m"
+            )
+            return goal_x, goal_y, yaw
+
+        if feasible_candidates:
+            goal_x, goal_y, yaw = min(
+                feasible_candidates,
+                key=lambda candidate: math.hypot(candidate[0] - robot_x, candidate[1] - robot_y)
+            )
+            self.get_logger().warn(
+                f"Approach candidates for {object_name}: total={len(approach_candidates)} "
+                f"feasible={len(feasible_candidates)} path_feasible=0, "
+                f"falling back to nearest map-feasible candidate=({goal_x:.4f}, {goal_y:.4f})"
+            )
+            return goal_x, goal_y, yaw
+
+        dx = obj_x - robot_x
+        dy = obj_y - robot_y
+        distance = math.hypot(dx, dy)
+        if distance < 1e-6:
+            yaw = 0.0
+            return obj_x, obj_y, yaw
+
+        ux = dx / distance
+        uy = dy / distance
+        goal_x = obj_x - ux * min(offset_m, max(distance - 0.05, 0.0))
+        goal_y = obj_y - uy * min(offset_m, max(distance - 0.05, 0.0))
+        yaw = math.atan2(dy, dx)
+        self.get_logger().warn(
+            f"Approach candidates for {object_name}: no feasible candidate found, "
+            f"falling back to line-of-sight offset goal=({goal_x:.4f}, {goal_y:.4f})"
+        )
+        return goal_x, goal_y, yaw
+
+    def generate_approach_candidates(self, obj_x, obj_y, offset_m, candidate_count):
+        candidates = []
+        for i in range(candidate_count):
+            angle = (2.0 * math.pi * i) / candidate_count
+            goal_x = obj_x + offset_m * math.cos(angle)
+            goal_y = obj_y + offset_m * math.sin(angle)
+            yaw = math.atan2(obj_y - goal_y, obj_x - goal_x)
+            candidates.append((goal_x, goal_y, yaw))
+        return candidates
+
+    def is_candidate_feasible(self, object_name: str, world_x, world_y):
+        map_msg = self.wait_for_map()
+        if map_msg is None:
+            return True
+
+        info = map_msg.info
+        resolution = info.resolution
+        origin_x = info.origin.position.x
+        origin_y = info.origin.position.y
+        width = info.width
+        height = info.height
+        grid_x = int((world_x - origin_x) / resolution)
+        grid_y = int((world_y - origin_y) / resolution)
+
+        if grid_x < 0 or grid_x >= width or grid_y < 0 or grid_y >= height:
+            return False
+
+        clearance_m = self.get_approach_clearance(object_name)
+        clearance_cells = max(0, int(math.ceil(clearance_m / resolution)))
+
+        for dy in range(-clearance_cells, clearance_cells + 1):
+            for dx in range(-clearance_cells, clearance_cells + 1):
+                if dx * dx + dy * dy > clearance_cells * clearance_cells:
+                    continue
+                xx = grid_x + dx
+                yy = grid_y + dy
+                if xx < 0 or xx >= width or yy < 0 or yy >= height:
+                    return False
+                cost = map_msg.data[yy * width + xx]
+                if cost < 0 or cost >= 50:
+                    return False
+        return True
+
+    def build_pose_stamped(self, x, y, yaw):
+        pose = PoseStamped()
+        pose.header.frame_id = "map"
+        pose.header.stamp = self.navigator.get_clock().now().to_msg()
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+        pose.pose.position.z = 0.0
+        pose.pose.orientation.z = math.sin(yaw / 2.0)
+        pose.pose.orientation.w = math.cos(yaw / 2.0)
+        return pose
+
+    def compute_path_length(self, path):
+        if not path.poses or len(path.poses) < 2:
+            return 0.0
+        length = 0.0
+        for prev_pose, next_pose in zip(path.poses[:-1], path.poses[1:]):
+            dx = next_pose.pose.position.x - prev_pose.pose.position.x
+            dy = next_pose.pose.position.y - prev_pose.pose.position.y
+            length += math.hypot(dx, dy)
+        return length
+
+    def wait_for_map(self, timeout_sec: float = 2.0):
+        if self.map_msg is not None:
+            return self.map_msg
+        end_time = time.time() + timeout_sec
+        while self.map_msg is None and time.time() < end_time:
+            time.sleep(0.05)
+        return self.map_msg
+
+    def get_approach_candidate_count(self, object_name: str) -> int:
+        default_count = int(
+            self.get_parameter('approach_candidate_count').get_parameter_value().integer_value
+        )
+        param_name = (
+            "approach_candidate_count_"
+            + object_name.lower().replace(" ", "_")
+        )
+        if self.has_parameter(param_name):
+            default_count = int(self.get_parameter(param_name).get_parameter_value().integer_value)
+        return min(max(default_count, 4), 16)
+
+    def get_approach_search_radii(self, object_name: str, default_offset: float):
+        param_name = (
+            "approach_offset_"
+            + object_name.lower().replace(" ", "_")
+            + "_search_radii"
+        )
+        if self.has_parameter(param_name):
+            value = self.get_parameter(param_name).value
+            if value:
+                return [float(radius) for radius in value]
+        return [default_offset]
+
+    def get_approach_clearance(self, object_name: str) -> float:
+        default_clearance = self.get_parameter(
+            'approach_clearance_m'
+        ).get_parameter_value().double_value
+        param_name = (
+            "approach_clearance_"
+            + object_name.lower().replace(" ", "_")
+            + "_m"
+        )
+        if self.has_parameter(param_name):
+            return self.get_parameter(param_name).get_parameter_value().double_value
+        return default_clearance
+
+    def get_approach_offset(self, object_name: str) -> float:
+        default_offset = self.get_parameter(
+            'approach_offset_m'
+        ).get_parameter_value().double_value
+        param_name = (
+            "approach_offset_"
+            + object_name.lower().replace(" ", "_")
+            + "_m"
+        )
+        if self.has_parameter(param_name):
+            return self.get_parameter(param_name).get_parameter_value().double_value
+        return default_offset
+
+    def cleanup(self):
+        if hasattr(self, 'tf_listener'):
+            try:
+                if hasattr(self.tf_listener, 'executor'):
+                    self.tf_listener.executor.shutdown(timeout_sec=0.5)
+                if hasattr(self.tf_listener, 'dedicated_listener_thread'):
+                    self.tf_listener.dedicated_listener_thread.join(timeout=0.5)
+                self.tf_listener.unregister()
+            except Exception:
+                pass
+
 
 
 
@@ -197,10 +398,14 @@ def main():
     json_file_path = os.path.join(package_share_dir, 'config', 'example_object_detection_vlm.json')
     node = SemanticNavNode(json_file_path)
 
-    user_input = input("Where do you want to go: ")
+    try:
-    node.navigate_to_object(user_input)
+        user_input = input("Where do you want to go: ")
-
+        node.navigate_to_object(user_input)
-    rclpy.shutdown()
+    finally:
+        node.cleanup()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
 
 
 if __name__ == "__main__":

src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py

@@ -1,27 +1,27 @@
 #!/usr/bin/env python3
 import rclpy
 from rclpy.node import Node
-from sensor_msgs.msg import Image
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
 from cv_bridge import CvBridge, CvBridgeError
 import torch
 import torch.nn.functional as F
 import torchvision
 from torchvision import transforms
-from PIL import Image as PILImage
 import clip
 import cv2
 import json
 import os
+import time
 from datetime import datetime
 import numpy as np
 from ament_index_python.packages import get_package_share_directory
 from tf2_ros import Buffer, TransformListener
-import tf2_geometry_msgs
-from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_pose
 from rclpy.duration import Duration
-from rclpy.time import Time
 from geometry_msgs.msg import PoseStamped
 from sensor_msgs.msg import Image, CameraInfo
+import tf2_geometry_msgs
+from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_pose
 
 
 
@@ -31,16 +31,24 @@ from sensor_msgs.msg import Image, CameraInfo
 class ClipDetectionNode(Node):
     def __init__(self):
         super().__init__('clip_detector')
+        self.declare_parameter('reset_records_on_startup', False)
+        self.declare_parameter('detection_interval_sec', 1.0)
+        self.declare_parameter('frcnn_min_score', 0.3)
+        self.declare_parameter('frcnn_top_k', 2)
         self.bridge = CvBridge()
         self.device = "cuda" if torch.cuda.is_available() else "cpu"
 
-        #camera init
+        # Camera cache: subscriber callbacks only keep the latest data.
         self.latest_depth = None
-        self.camera_info = None
-        self.latest_stamp = None
         self.latest_depth_stamp = None
-        self.last_tf_warn_time = None
+        self.camera_info = None
+        self.latest_camera_info_stamp = None
+        self.latest_stamp = None
         self.latest_rgb = None
+        self.img_frame = "realsense_depth_frame"
+        self.processing_frame = False
+        self.sensor_callback_group = ReentrantCallbackGroup()
+        self.timer_callback_group = MutuallyExclusiveCallbackGroup()
         self.tf_buffer = Buffer(cache_time=Duration(seconds=10))
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
@@ -51,32 +59,31 @@ class ClipDetectionNode(Node):
 
         # Labels and score tracking
         self.labels = ["Refrigerator","water dispenser", "sofa", "white toilet",  "office chair with wheels"]
-
+        
-        # Save directly to vlm_nav_pkg config so semantic_nav reads the same file.
+        # Initialize score tracking - save directly to vlm_nav_pkg config
+        # so semantic_nav reads the same file without manual copy
         try:
             nav_pkg_dir = get_package_share_directory('vlm_nav_pkg')
-            default_records_file = os.path.join(
+            self.score_records_file = os.path.join(
                 nav_pkg_dir, 'config', 'example_object_detection_vlm.json'
             )
         except Exception:
             self.get_logger().warn(
                 "vlm_nav_pkg not found, saving JSON to current directory"
             )
-            default_records_file = "object_detection_vlm.json"
+            self.score_records_file = "object_detection_vlm.json"
-
+        reset_records = self.get_parameter(
-        self.declare_parameter("score_records_file", default_records_file)
+            'reset_records_on_startup'
-        self.declare_parameter("reset_records_on_startup", False)
+        ).get_parameter_value().bool_value
-        configured_file = self.get_parameter("score_records_file").value
+        if reset_records:
-        self.score_records_file = configured_file if configured_file else default_records_file
+            self.score_records = {
-        self.reset_records_on_startup = self.get_parameter("reset_records_on_startup").value
+                label: {"highest_score": 0.0, "last_detected": None, "position": None}
-
+                for label in self.labels
-        if self.reset_records_on_startup:
+            }
-            self.score_records = self.get_empty_score_records()
             self.save_score_records()
             self.get_logger().info(f"Score records reset: {self.score_records_file}")
         else:
             self.score_records = self.load_score_records()
-            self.save_score_records()
             self.get_logger().info(f"Score records loaded: {self.score_records_file}")
         
         # Subscribers & Publishers
@@ -86,84 +93,53 @@ class ClipDetectionNode(Node):
             Image,
             "/intel_realsense_r200_rgb/image_raw",
             self.rgb_callback,
-            10
+            10,
+            callback_group=self.sensor_callback_group
         )
         self.depth_sub = self.create_subscription(
             Image,
             "/intel_realsense_r200_depth/depth/image_raw",
             self.depth_callback,
-            10
+            10,
+            callback_group=self.sensor_callback_group
         )
         self.camera_info_sub = self.create_subscription(
             CameraInfo, "/intel_realsense_r200_depth/camera_info",
-            self.camera_info_callback, 10)
+            self.camera_info_callback, 10,
+            callback_group=self.sensor_callback_group)
         self.annotated_pub = self.create_publisher(Image, "/percep/annotated_image", 10)
+        interval_sec = self.get_parameter(
+            'detection_interval_sec'
+        ).get_parameter_value().double_value
+        self.detection_timer = self.create_timer(
+            interval_sec,
+            self.process_frame,
+            callback_group=self.timer_callback_group,
+        )
+        self.get_logger().info(
+            f"Detection timer enabled: processing latest frame every {interval_sec:.2f}s"
+        )
 
 
     def camera_info_callback(self, msg):
         self.camera_info = msg
-        self.img_frame = "realsense_depth_frame"
+        self.latest_camera_info_stamp = msg.header.stamp
-        self.get_logger().info(f"Camera info received, using frame: {self.img_frame}")
+        if msg.header.frame_id:
+            self.img_frame = msg.header.frame_id
 
         
 
-    def get_empty_score_records(self):
-        return {
-            label: {"highest_score": 0.0, "last_detected": None, "position": None}
-            for label in self.labels
-        }
-
     def load_score_records(self):
-        """Load existing score records and normalize missing labels/fields."""
+        """Load existing score records or create new ones"""
-        empty_records = self.get_empty_score_records()
+        if os.path.exists(self.score_records_file):
-        if not os.path.exists(self.score_records_file):
+            with open(self.score_records_file, 'r') as f:
-            self.get_logger().warn(
+                try:
-                f"Score records file not found, creating new one: {self.score_records_file}"
+                    return json.load(f)
-            )
+                except json.JSONDecodeError:
-            return empty_records
+                    self.get_logger().warn("Score file corrupted, creating new one")
-
+        
-        with open(self.score_records_file, 'r') as f:
+        # Initialize with empty records
-            try:
+        return {label: {"highest_score": 0.0, "last_detected": None, "position":None} for label in self.labels}
-                loaded = json.load(f)
-            except json.JSONDecodeError:
-                self.get_logger().warn("Score file corrupted, creating new one")
-                return empty_records
-
-        if not isinstance(loaded, dict):
-            self.get_logger().warn("Score file format is invalid, creating new one")
-            return empty_records
-
-        def normalize_record(record):
-            if not isinstance(record, dict):
-                record = {}
-
-            highest_score = record.get("highest_score", 0.0)
-            try:
-                highest_score = float(highest_score)
-            except (TypeError, ValueError):
-                highest_score = 0.0
-            highest_score = max(0.0, highest_score)
-
-            last_detected = record.get("last_detected")
-            position = record.get("position")
-            if position is not None and not isinstance(position, dict):
-                position = None
-
-            return {
-                "highest_score": highest_score,
-                "last_detected": last_detected,
-                "position": position
-            }
-
-        normalized = {}
-        for label, record in loaded.items():
-            normalized[label] = normalize_record(record)
-
-        for label in self.labels:
-            if label not in normalized:
-                normalized[label] = empty_records[label]
-
-        return normalized
 
     def save_score_records(self):
         """Save current score records to file"""
@@ -171,40 +147,40 @@ class ClipDetectionNode(Node):
             json.dump(self.score_records, f, indent=4)
 
     def update_score_records(self, label, score, position=None):
-        """Keep highest score per label; equal score is treated as an update."""
+        """Update records when confidence matches or exceeds the best score."""
         current_time = datetime.now().isoformat()
-
+        
         if label not in self.score_records:
             self.score_records[label] = {
-                "highest_score": 0.0,
+                "highest_score": score,
-                "last_detected": None,
+                "last_detected": current_time,
-                "position": None
+                "position": position
             }
-
-        record = self.score_records[label]
-        updated = False
-        if score >= record["highest_score"]:
-            # Equal score is also accepted so records can refresh over time.
-            record["highest_score"] = score
-            if position is not None:
-                record["position"] = position
-            updated = True
-
-        record["last_detected"] = current_time
-        if updated:
             self.save_score_records()
-
+            return True
+        
+        updated = False
+        if score >= self.score_records[label]["highest_score"]:
+            # Refresh the best score and associated position on ties or improvements.
+            self.score_records[label]["highest_score"] = score
+            if position is not None:
+                self.score_records[label]["position"] = position
+            updated = True
+        
+        # Always refresh timestamp (even without position/score change)
+        self.score_records[label]["last_detected"] = current_time
+        self.save_score_records()
+        
         return updated
 
 
     def rgb_callback(self, msg):
         try:
             self.latest_rgb = self.bridge.imgmsg_to_cv2(msg, "bgr8")
-            self.latest_stamp = msg.header.stamp  # <-- Correct assignment
+            self.latest_stamp = msg.header.stamp
         except CvBridgeError as e:
             self.get_logger().error(f"CvBridge error: {e}")
             return
-        self.process_frame()
 
 
     def depth_callback(self, msg):
@@ -214,85 +190,87 @@ class ClipDetectionNode(Node):
         except CvBridgeError as e:
             self.get_logger().error(f"Depth CvBridge error: {e}")
             return
-        self.process_frame()
 
     def process_frame(self):
-        if self.latest_rgb is None or self.latest_depth is None:
+        if self.processing_frame:
             return
 
-        cv_img = self.latest_rgb.copy()
+        if self.latest_rgb is None or self.latest_depth is None or self.camera_info is None:
-        image_tensor = transforms.ToTensor()(cv_img)
+            return
 
-        with torch.no_grad():
+        if self.latest_stamp is None or self.latest_depth_stamp is None:
-            frcnn_output = self.frcnn_model([image_tensor])[0]
+            return
-        candidate_boxes = frcnn_output['boxes']
 
-        best_labels_per_box = {}  # key: box index, value: (label, score)
+        self.processing_frame = True
+        start_time = time.perf_counter()
+        try:
+            cv_img = self.latest_rgb.copy()
+            depth_img = self.latest_depth.copy()
+            frame_stamp = self.latest_depth_stamp
+            camera_info = self.camera_info
+            image_tensor = transforms.ToTensor()(cv_img)
+            frcnn_min_score = self.get_parameter(
+                'frcnn_min_score'
+            ).get_parameter_value().double_value
+            frcnn_top_k = self.get_parameter(
+                'frcnn_top_k'
+            ).get_parameter_value().integer_value
 
-        for i, box in enumerate(candidate_boxes):
+            with torch.no_grad():
-            max_score = -1
+                frcnn_output = self.frcnn_model([image_tensor])[0]
-            best_label = None
+            candidate_boxes = frcnn_output['boxes']
-            for label in self.labels:
+            candidate_scores = frcnn_output['scores']
-                _, score = self.match_label_box(label, image_tensor, [box])
+            total_candidate_count = len(candidate_boxes)
-                if score > max_score:
-                    max_score = score
-                    best_label = label
 
-            if max_score >= 0.25 and best_label is not None:
+            filtered_indices = [
-                best_labels_per_box[i] = (best_label, max_score)
+                i for i, score in enumerate(candidate_scores)
-                # Initialize record_updated as False first
+                if float(score) >= frcnn_min_score
-                record_updated = False
+            ]
-                position = None
+            filtered_indices.sort(key=lambda i: float(candidate_scores[i]), reverse=True)
+            selected_indices = filtered_indices[:frcnn_top_k]
+            candidate_boxes = [candidate_boxes[i] for i in selected_indices]
+            self.get_logger().info(
+                f"FRCNN candidates: total={total_candidate_count} "
+                f"filtered={len(filtered_indices)} top_k={frcnn_top_k} "
+                f"selected={len(candidate_boxes)}"
+            )
+
+            best_labels_per_box = {}  # key: box index, value: (label, score)
+
+            for i, box in enumerate(candidate_boxes):
+                max_score = -1
+                best_label = None
+                for label in self.labels:
+                    _, score = self.match_label_box(label, image_tensor, [box])
+                    if score > max_score:
+                        max_score = score
+                        best_label = label
+
+                if max_score >= 0.25 and best_label is not None:
+                    best_labels_per_box[i] = (best_label, max_score)
+                    is_new_record = False
+                    position = None
 
-                if self.latest_depth is not None and self.camera_info is not None:
                     box_index = i
                     box = candidate_boxes[box_index]
                     x_min, y_min, x_max, y_max = map(int, box)
                     x_center = (x_min + x_max) // 2
                     y_center = (y_min + y_max) // 2
-                    depth = float(self.latest_depth[y_center, x_center])
+                    depth = float(depth_img[y_center, x_center])
                     if np.isnan(depth) or depth <= 0.0:
                         continue
 
-                    fx = self.camera_info.k[0]
+                    fx = camera_info.k[0]
-                    fy = self.camera_info.k[4]
+                    fy = camera_info.k[4]
-                    cx = self.camera_info.k[2]
+                    cx = camera_info.k[2]
-                    cy = self.camera_info.k[5]
+                    cy = camera_info.k[5]
 
                     X = (x_center - cx) * depth / fx
                     Y = (y_center - cy) * depth / fy
                     Z = depth
 
-                    stamp_msg = self.latest_depth_stamp if self.latest_depth_stamp is not None else self.latest_stamp
-                    if stamp_msg is None:
-                        continue
-                    query_time = Time.from_msg(
-                        stamp_msg, clock_type=self.get_clock().clock_type
-                    )
-                    tf_stamp_msg = stamp_msg
-                    has_tf_at_stamp = self.tf_buffer.can_transform(
-                        "map", self.img_frame, query_time, timeout=Duration(seconds=0.05)
-                    )
-                    if not has_tf_at_stamp:
-                        latest_query_time = Time(clock_type=self.get_clock().clock_type)
-                        has_tf_latest = self.tf_buffer.can_transform(
-                            "map", self.img_frame, latest_query_time, timeout=Duration(seconds=0.05)
-                        )
-                        if not has_tf_latest:
-                            now_time = self.get_clock().now()
-                            if (
-                                self.last_tf_warn_time is None
-                                or (now_time - self.last_tf_warn_time).nanoseconds > 2_000_000_000
-                            ):
-                                self.get_logger().warn(
-                                    "TF unavailable at sensor stamp and latest time; skip this detection frame."
-                                )
-                                self.last_tf_warn_time = now_time
-                            continue
-                        tf_stamp_msg = latest_query_time.to_msg()
-
                     pose = PoseStamped()
-                    pose.header.stamp = tf_stamp_msg
+                    pose.header.stamp = frame_stamp
                     pose.header.frame_id = self.img_frame
                     pose.pose.position.x = X
                     pose.pose.position.y = Y
@@ -302,7 +280,7 @@ class ClipDetectionNode(Node):
                     # TF transform: camera frame -> map frame (semantic_nav 需要 map 坐标)
                     try:
                         ob2map = self.tf_buffer.transform(
-                            pose, "map", timeout=Duration(seconds=0.05)
+                            pose, "map", timeout=Duration(seconds=1.0)
                         )
 
                         self.pose_pub.publish(ob2map)
@@ -316,33 +294,28 @@ class ClipDetectionNode(Node):
                             "y": round(ob2map.pose.position.y, 4),
                             "z": round(ob2map.pose.position.z, 4)
                         }
-                        record_updated = self.update_score_records(best_label, max_score, position=position)
+                        is_new_record = self.update_score_records(best_label, max_score, position=position)
                     except Exception as e:
                         self.get_logger().error(f"TF transform to map failed: {e}")
-                        record_updated = False
+                        is_new_record = False
 
+                    log_msg = f"{best_label} detected with confidence score {max_score:.2f}"
+                    if is_new_record:
+                        log_msg += " (NEW RECORD!)"
+                    self.get_logger().info(log_msg)
 
-                # Logging
+            matched_boxes = [candidate_boxes[i] for i in best_labels_per_box.keys()]
-                log_msg = f"{best_label} detected with confidence score {max_score:.2f}"
+            matched_labels = [f"{lbl} ({score:.2f})" for lbl, score in best_labels_per_box.values()]
-                if record_updated:
-                    log_msg += " (BEST SCORE RECORD UPDATED)"
-                self.get_logger().info(log_msg)
 
-
+            annotated_img = self.draw_boxes(cv_img, matched_boxes, matched_labels)
-###############################################
+            try:
-
+                self.annotated_pub.publish(self.bridge.cv2_to_imgmsg(annotated_img, encoding="bgr8"))
-        matched_boxes = [candidate_boxes[i] for i in best_labels_per_box.keys()]
+            except CvBridgeError as e:
-        matched_labels = [f"{lbl} ({score:.2f})" for lbl, score in best_labels_per_box.values()]
+                self.get_logger().error(f"Publish error: {e}")
-
+        finally:
-        annotated_img = self.draw_boxes(cv_img, matched_boxes, matched_labels)
+            elapsed_ms = (time.perf_counter() - start_time) * 1000.0
-        try:
+            self.get_logger().info(f"Detection cycle took {elapsed_ms:.1f} ms")
-            self.annotated_pub.publish(self.bridge.cv2_to_imgmsg(annotated_img, encoding="bgr8"))
+            self.processing_frame = False
-        except CvBridgeError as e:
-            self.get_logger().error(f"Publish error: {e}")
-
-        self.latest_rgb, self.latest_depth = None, None
-        self.latest_stamp = None  # 同步清空时间戳，避免TF查询残留旧stamp
-        self.latest_depth_stamp = None
         
     def match_label_box(self, label, image_tensor, boxes):
         max_sim, best_box = -1, None
@@ -369,12 +342,15 @@ class ClipDetectionNode(Node):
 def main(args=None):
     rclpy.init(args=args)
     node = ClipDetectionNode()
+    executor = MultiThreadedExecutor(num_threads=2)
+    executor.add_node(node)
     try:
-        rclpy.spin(node)
+        executor.spin()
     except KeyboardInterrupt:
         node.get_logger().info("KeyboardInterrupt received, shutting down...")
     finally:
         node.save_score_records()
+        executor.shutdown()
         node.destroy_node()
         if rclpy.ok():
             rclpy.shutdown()