update

README.md

@@ -69,8 +69,10 @@ ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_vie
 ```
 2.   新开终端，执行环境生效后启动vlm_perception_pkg的语义检测节点，开始目标语义标注并构建语义记忆库，在机器人导航过程中，节点实时检测环境中的目标并生成语义标注结果，保存JSON 格式的语义记忆库到指定路径。
 ```bash
-ros2 run vlm_perception_pkg vlm_detection
+ros2 run vlm_perception_pkg vlm_detection --ros-args -p use_sim_time:=true
-# ros2 run vlm_perception_pkg vlm_detection --ros-args -p use_sim_time:=true -p reset_records_on_startup:=true # 重置检测记录
+# 默认：启动时加载历史 JSON（同一个语义记忆库文件）
+# 可选：启动时重置语义记忆库，避免历史最高分长期不更新
+# ros2 run vlm_perception_pkg vlm_detection --ros-args -p use_sim_time:=true -p reset_records_on_startup:=true
 ```
 3.   加载生成的语义记忆库文件（in /workspace/src/vlm_nav_pkg/config），通过 CLIP 将语义标签编码为shared embedding space，当用户输入自然语言查询时，系统匹配最接近的已知目标，提取其坐标并发送至 Nav2，实现基于自然语言的语义导航。以下是验证步骤：
 4.   重新启动仿真， Nav2导航栈， AMCL节点，rviz

src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py

@@ -13,9 +13,7 @@ import cv2
 import json
 import os
 from datetime import datetime
-from math import isfinite
 import numpy as np
-import yaml
 from ament_index_python.packages import get_package_share_directory
 from tf2_ros import Buffer, TransformListener
 import tf2_geometry_msgs
@@ -36,54 +34,14 @@ class ClipDetectionNode(Node):
         self.bridge = CvBridge()
         self.device = "cuda" if torch.cuda.is_available() else "cpu"
 
-        # Behavior controls for semantic memory persistence and position quality.
-        self.declare_parameter('reset_records_on_startup', False)
-        self.declare_parameter('position_abs_limit_xy', 50.0)
-        self.declare_parameter('position_z_min', -2.0)
-        self.declare_parameter('position_z_max', 5.0)
-        self.declare_parameter('camera_frame_fallback', 'realsense_depth_frame')
-        self.declare_parameter(
-            'map_yaml_path',
-            os.environ.get('NAV2_MAP_PATH', '')
-        )
-        self.declare_parameter('best_position_confirm_frames', 3)
-        self.declare_parameter('best_position_confirm_radius', 0.8)
-        self.reset_records_on_startup = bool(
-            self.get_parameter('reset_records_on_startup').value
-        )
-        self.position_abs_limit_xy = float(
-            self.get_parameter('position_abs_limit_xy').value
-        )
-        self.position_z_min = float(self.get_parameter('position_z_min').value)
-        self.position_z_max = float(self.get_parameter('position_z_max').value)
-        self.camera_frame_fallback = str(
-            self.get_parameter('camera_frame_fallback').value
-        )
-        self.map_yaml_path = str(self.get_parameter('map_yaml_path').value).strip()
-        self.best_position_confirm_frames = int(
-            self.get_parameter('best_position_confirm_frames').value
-        )
-        self.best_position_confirm_radius = float(
-            self.get_parameter('best_position_confirm_radius').value
-        )
-        if not self.camera_frame_fallback:
-            self.camera_frame_fallback = 'realsense_depth_frame'
-        if self.best_position_confirm_frames < 1:
-            self.best_position_confirm_frames = 1
-        if self.best_position_confirm_radius <= 0.0:
-            self.best_position_confirm_radius = 0.8
-
         #camera init
         self.latest_depth = None
         self.camera_info = None
         self.latest_stamp = None
-        self.latest_rgb = None
         self.latest_depth_stamp = None
-        self.img_frame = self.camera_frame_fallback
+        self.last_tf_warn_time = None
-        self.last_tf_warn = ""
+        self.latest_rgb = None
-        self.last_frame_warn = ""
+        self.tf_buffer = Buffer(cache_time=Duration(seconds=10))
-        # Keep a longer TF history to tolerate localization TF gaps.
-        self.tf_buffer = Buffer(cache_time=Duration(seconds=120))
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
 
@@ -93,39 +51,33 @@ class ClipDetectionNode(Node):
 
         # Labels and score tracking
         self.labels = ["Refrigerator","water dispenser", "sofa", "white toilet",  "office chair with wheels"]
-        self.best_position_candidates = {label: [] for label in self.labels}
-        self.map_bounds = self._load_map_bounds()
 
-        # Initialize score tracking - save directly to vlm_nav_pkg config
+        # Save directly to vlm_nav_pkg config so semantic_nav reads the same file.
-        # so semantic_nav reads the same file without manual copy
         try:
             nav_pkg_dir = get_package_share_directory('vlm_nav_pkg')
-            self.score_records_file = os.path.join(
+            default_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"
             )
-            self.score_records_file = os.path.join(
+            default_records_file = "object_detection_vlm.json"
-                nav_pkg_dir, 'config', 'object_detection_vlm.json'
+
-            )
+        self.declare_parameter("score_records_file", default_records_file)
+        self.declare_parameter("reset_records_on_startup", False)
+        configured_file = self.get_parameter("score_records_file").value
+        self.score_records_file = configured_file if configured_file else default_records_file
+        self.reset_records_on_startup = self.get_parameter("reset_records_on_startup").value
+
         if self.reset_records_on_startup:
-            self.score_records = {
+            self.score_records = self.get_empty_score_records()
-                label: self._empty_record()
-                for label in self.labels
-            }
             self.save_score_records()
-            self.get_logger().info(
+            self.get_logger().info(f"Score records reset: {self.score_records_file}")
-                f"Score records reset: {self.score_records_file}"
-            )
         else:
             self.score_records = self.load_score_records()
-            # Persist normalized schema for legacy files.
             self.save_score_records()
-            self.get_logger().info(
+            self.get_logger().info(f"Score records loaded: {self.score_records_file}")
-                f"Score records loaded: {self.score_records_file}"
-            )
         
         # Subscribers & Publishers
         self.pose_pub = self.create_publisher(PoseStamped, "/object_in_map", 10)
@@ -150,196 +102,66 @@ class ClipDetectionNode(Node):
 
     def camera_info_callback(self, msg):
         self.camera_info = msg
-        incoming_frame = (msg.header.frame_id or "").strip()
+        self.img_frame = "realsense_depth_frame"
-        if not incoming_frame:
+        self.get_logger().info(f"Camera info received, using frame: {self.img_frame}")
-            return
-        if incoming_frame == self.img_frame:
-            return
-
-        # Gz bridge may publish scoped names like
-        # burger/realsense_link/intel_realsense_r200_depth which are not in TF.
-        # Keep the ROS TF frame fallback in that case.
-        if "/" in incoming_frame:
-            warn_msg = (
-                f"Ignoring scoped camera frame '{incoming_frame}', "
-                f"using fallback '{self.camera_frame_fallback}'"
-            )
-            if warn_msg != self.last_frame_warn:
-                self.get_logger().warn(warn_msg)
-                self.last_frame_warn = warn_msg
-            self.img_frame = self.camera_frame_fallback
-            return
-
-        self.img_frame = incoming_frame
-        self.get_logger().info(f"Camera info frame updated: {self.img_frame}")
 
         
 
-    def _empty_record(self):
+    def get_empty_score_records(self):
         return {
-            "highest_score": 0.0,
+            label: {"highest_score": 0.0, "last_detected": None, "position": None}
-            "last_detected": None,
+            for label in self.labels
-            # Backward-compatible field consumed by semantic_nav.
-            "position": None,
-            # New fields:
-            "best_position": None,
-            "last_position": None,
-        }
-
-    def _resolve_map_yaml_path(self):
-        path = os.path.expanduser(self.map_yaml_path) if self.map_yaml_path else ''
-        if path and os.path.isfile(path):
-            return path
-        try:
-            tb3_dir = get_package_share_directory('turtlebot3_gazebo')
-            default_path = os.path.join(tb3_dir, 'map', 'office_map.yaml')
-            if os.path.isfile(default_path):
-                return default_path
-        except Exception:
-            pass
-        return ''
-
-    def _load_map_bounds(self):
-        map_yaml = self._resolve_map_yaml_path()
-        if not map_yaml:
-            self.get_logger().warn(
-                "Map bounds disabled: map_yaml_path not found. "
-                "Set NAV2_MAP_PATH or parameter map_yaml_path."
-            )
-            return None
-        try:
-            with open(map_yaml, 'r') as f:
-                map_cfg = yaml.safe_load(f)
-            resolution = float(map_cfg['resolution'])
-            origin = map_cfg['origin']
-            origin_x = float(origin[0])
-            origin_y = float(origin[1])
-            image_rel = str(map_cfg['image'])
-            image_path = image_rel
-            if not os.path.isabs(image_path):
-                image_path = os.path.join(os.path.dirname(map_yaml), image_rel)
-            map_img = cv2.imread(image_path, cv2.IMREAD_UNCHANGED)
-            if map_img is None:
-                raise RuntimeError(f"cannot open map image: {image_path}")
-            height, width = map_img.shape[:2]
-            bounds = {
-                'min_x': origin_x,
-                'max_x': origin_x + width * resolution,
-                'min_y': origin_y,
-                'max_y': origin_y + height * resolution,
-                'map_yaml': map_yaml,
-            }
-            self.get_logger().info(
-                "Map bounds loaded: "
-                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"Map bounds disabled due to load error: {e}"
-            )
-            return None
-
-    def _is_in_map_bounds(self, x, y):
-        if self.map_bounds is None:
-            return True
-        return (
-            self.map_bounds['min_x'] <= x <= self.map_bounds['max_x']
-            and self.map_bounds['min_y'] <= y <= self.map_bounds['max_y']
-        )
-
-    def _positions_consistent(self, positions):
-        if len(positions) < self.best_position_confirm_frames:
-            return False
-        center_x = float(np.median([p['x'] for p in positions]))
-        center_y = float(np.median([p['y'] for p in positions]))
-        max_dist = 0.0
-        for p in positions:
-            dx = p['x'] - center_x
-            dy = p['y'] - center_y
-            dist = float(np.hypot(dx, dy))
-            if dist > max_dist:
-                max_dist = dist
-        return max_dist <= self.best_position_confirm_radius
-
-    def _median_position(self, positions):
-        if not positions:
-            return None
-        return {
-            "x": round(float(np.median([p['x'] for p in positions])), 4),
-            "y": round(float(np.median([p['y'] for p in positions])), 4),
-            "z": round(float(np.median([p['z'] for p in positions])), 4),
-        }
-
-    def _sanitize_position(self, position):
-        """Return normalized position dict or None if invalid."""
-        if position is None:
-            return None
-        try:
-            x = float(position["x"])
-            y = float(position["y"])
-            z = float(position.get("z", 0.0))
-        except Exception:
-            return None
-
-        if not (isfinite(x) and isfinite(y) and isfinite(z)):
-            return None
-        if abs(x) > self.position_abs_limit_xy or abs(y) > self.position_abs_limit_xy:
-            return None
-        if z < self.position_z_min or z > self.position_z_max:
-            return None
-        if not self._is_in_map_bounds(x, y):
-            return None
-
-        return {
-            "x": round(x, 4),
-            "y": round(y, 4),
-            "z": round(z, 4),
         }
 
     def load_score_records(self):
-        """Load existing score records and normalize to current schema."""
+        """Load existing score records and normalize missing labels/fields."""
-        raw_records = {}
+        empty_records = self.get_empty_score_records()
-        if os.path.exists(self.score_records_file):
+        if not os.path.exists(self.score_records_file):
+            self.get_logger().warn(
+                f"Score records file not found, creating new one: {self.score_records_file}"
+            )
+            return empty_records
+
         with open(self.score_records_file, 'r') as f:
             try:
-                    raw_records = json.load(f)
+                loaded = json.load(f)
             except json.JSONDecodeError:
                 self.get_logger().warn("Score file corrupted, creating new one")
-                    raw_records = {}
+                return empty_records
-        if not isinstance(raw_records, dict):
-            self.get_logger().warn("Score file format invalid, recreating records")
-            raw_records = {}
 
-        normalized = {}
+        if not isinstance(loaded, dict):
-        for label in self.labels:
+            self.get_logger().warn("Score file format is invalid, creating new one")
-            src = raw_records.get(label, {})
+            return empty_records
-            dst = self._empty_record()
 
-            highest_score = src.get("highest_score", 0.0)
+        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 Exception:
+            except (TypeError, ValueError):
                 highest_score = 0.0
-            if not isfinite(highest_score):
+            highest_score = max(0.0, highest_score)
-                highest_score = 0.0
-            dst["highest_score"] = max(0.0, highest_score)
-            dst["last_detected"] = src.get("last_detected")
 
-            legacy_pos = self._sanitize_position(src.get("position"))
+            last_detected = record.get("last_detected")
-            best_pos = self._sanitize_position(src.get("best_position"))
+            position = record.get("position")
-            last_pos = self._sanitize_position(src.get("last_position"))
+            if position is not None and not isinstance(position, dict):
+                position = None
 
-            if best_pos is None:
+            return {
-                best_pos = legacy_pos
+                "highest_score": highest_score,
-            if last_pos is None:
+                "last_detected": last_detected,
-                last_pos = legacy_pos
+                "position": position
+            }
 
-            dst["best_position"] = best_pos
+        normalized = {}
-            dst["last_position"] = last_pos
+        for label, record in loaded.items():
-            dst["position"] = best_pos
+            normalized[label] = normalize_record(record)
-            normalized[label] = dst
+
+        for label in self.labels:
+            if label not in normalized:
+                normalized[label] = empty_records[label]
 
         return normalized
 
@@ -349,75 +171,30 @@ class ClipDetectionNode(Node):
             json.dump(self.score_records, f, indent=4)
 
     def update_score_records(self, label, score, position=None):
-        """Update semantic memory with dual-track positions.
+        """Keep highest score per label; equal score is treated as an update."""
-
-        - best_position: saved when confidence reaches a new high score.
-        - last_position: latest valid position, regardless of score.
-        - position: backward-compatible mirror of best_position.
-        """
         current_time = datetime.now().isoformat()
 
         if label not in self.score_records:
-            self.score_records[label] = self._empty_record()
+            self.score_records[label] = {
+                "highest_score": 0.0,
+                "last_detected": None,
+                "position": None
+            }
 
         record = self.score_records[label]
-        changed = False
+        updated = False
-        new_best = False
+        if score >= record["highest_score"]:
-
+            # Equal score is also accepted so records can refresh over time.
-        # Always refresh timestamp when object is observed.
+            record["highest_score"] = score
-        record["last_detected"] = current_time
-
-        try:
-            score = float(score)
-        except Exception:
-            score = 0.0
-        if not isfinite(score):
-            score = 0.0
-
-        normalized_position = self._sanitize_position(position)
-        if normalized_position is not None:
-            if record.get("last_position") != normalized_position:
-                record["last_position"] = normalized_position
-                changed = True
-
-            candidates = self.best_position_candidates.setdefault(label, [])
-            candidates.append({
-                "position": normalized_position,
-                "score": score,
-            })
-            if len(candidates) > self.best_position_confirm_frames:
-                candidates.pop(0)
-
-            highest_score = float(record.get("highest_score", 0.0))
-            recent_positions = [c["position"] for c in candidates]
-            if (
-                len(candidates) >= self.best_position_confirm_frames
-                and self._positions_consistent(recent_positions)
-            ):
-                candidate_score = max(float(c["score"]) for c in candidates)
-                if candidate_score > highest_score:
-                    median_pos = self._median_position(recent_positions)
-                    record["highest_score"] = candidate_score
-                    record["best_position"] = median_pos
-                    record["position"] = median_pos
-                    changed = True
-                    new_best = True
-            elif record.get("position") != record.get("best_position"):
-                record["position"] = record.get("best_position")
-                changed = True
-        else:
             if position is not None:
-                self.get_logger().warn(
+                record["position"] = position
-                    f"Invalid map position dropped for '{label}': {position}"
+            updated = True
-                )
-            if record.get("position") != record.get("best_position"):
-                record["position"] = record.get("best_position")
-                changed = True
 
-        if changed:
+        record["last_detected"] = current_time
+        if updated:
             self.save_score_records()
 
-        return new_best
+        return updated
 
 
     def rgb_callback(self, msg):
@@ -463,8 +240,8 @@ class ClipDetectionNode(Node):
 
             if max_score >= 0.25 and best_label is not None:
                 best_labels_per_box[i] = (best_label, max_score)
-                # Initialize is_new_record as False first
+                # Initialize record_updated as False first
-                is_new_record = False
+                record_updated = False
                 position = None
 
                 if self.latest_depth is not None and self.camera_info is not None:
@@ -486,8 +263,36 @@ class ClipDetectionNode(Node):
                     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 = Time().to_msg()  # 零时间=使用最新可用TF
+                    pose.header.stamp = tf_stamp_msg
                     pose.header.frame_id = self.img_frame
                     pose.pose.position.x = X
                     pose.pose.position.y = Y
@@ -497,57 +302,8 @@ class ClipDetectionNode(Node):
                     # TF transform: camera frame -> map frame (semantic_nav 需要 map 坐标)
                     try:
                         ob2map = self.tf_buffer.transform(
-                            pose, "map", timeout=Duration(seconds=1.0)
+                            pose, "map", timeout=Duration(seconds=0.05)
                         )
-                    except Exception as e:
-                        err_msg = str(e)
-                        # Fallback path:
-                        # evaluate source->odom and map->odom independently at latest time.
-                        # This avoids "latest common time" failures when map->odom is sparse.
-                        if "source_frame does not exist" in err_msg:
-                            try:
-                                pose.header.frame_id = self.camera_frame_fallback
-                                ob2map = self.tf_buffer.transform(
-                                    pose, "map", timeout=Duration(seconds=0.2)
-                                )
-                                warn_msg = (
-                                    "Recovered TF by forcing fallback frame: "
-                                    f"{self.camera_frame_fallback}"
-                                )
-                                if warn_msg != self.last_tf_warn:
-                                    self.get_logger().warn(warn_msg)
-                                    self.last_tf_warn = warn_msg
-                            except Exception as fallback_err:
-                                self.get_logger().error(
-                                    f"TF transform to map failed: {fallback_err}"
-                                )
-                                is_new_record = False
-                                continue
-                        elif "extrapolation into the past" in err_msg:
-                            try:
-                                ob2map = self.tf_buffer.transform_full(
-                                    pose,
-                                    "map",
-                                    Time(),
-                                    "odom",
-                                    timeout=Duration(seconds=0.2)
-                                )
-                                warn_msg = (
-                                    "Recovered TF with transform_full fallback via fixed frame 'odom'"
-                                )
-                                if warn_msg != self.last_tf_warn:
-                                    self.get_logger().warn(warn_msg)
-                                    self.last_tf_warn = warn_msg
-                            except Exception as fallback_err:
-                                self.get_logger().error(
-                                    f"TF transform to map failed: {fallback_err}"
-                                )
-                                is_new_record = False
-                                continue
-                        else:
-                            self.get_logger().error(f"TF transform to map failed: {e}")
-                            is_new_record = False
-                            continue
 
                         self.pose_pub.publish(ob2map)
                         self.get_logger().info(
@@ -556,17 +312,20 @@ class ClipDetectionNode(Node):
                         )
 
                         position = {
-                        "x": ob2map.pose.position.x,
+                            "x": round(ob2map.pose.position.x, 4),
-                        "y": ob2map.pose.position.y,
+                            "y": round(ob2map.pose.position.y, 4),
-                        "z": ob2map.pose.position.z
+                            "z": round(ob2map.pose.position.z, 4)
                         }
-                    is_new_record = self.update_score_records(best_label, max_score, position=position)
+                        record_updated = 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
 
 
                 # Logging
                 log_msg = f"{best_label} detected with confidence score {max_score:.2f}"
-                if is_new_record:
+                if record_updated:
-                    log_msg += " (NEW RECORD!)"
+                    log_msg += " (BEST SCORE RECORD UPDATED)"
                 self.get_logger().info(log_msg)