diff --git a/README.md b/README.md index 60880d6..3ae7a35 100644 --- a/README.md +++ b/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 -p reset_records_on_startup:=true # 重置检测记录 +ros2 run vlm_perception_pkg vlm_detection --ros-args -p use_sim_time:=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 diff --git a/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py b/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py index e0ee887..20dc727 100755 --- a/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py +++ b/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 = "" - self.last_frame_warn = "" - # Keep a longer TF history to tolerate localization TF gaps. - self.tf_buffer = Buffer(cache_time=Duration(seconds=120)) + self.last_tf_warn_time = None + self.latest_rgb = None + self.tf_buffer = Buffer(cache_time=Duration(seconds=10)) 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 - # so semantic_nav reads the same file without manual copy + + # Save directly to vlm_nav_pkg config so semantic_nav reads the same file. 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( - nav_pkg_dir, 'config', 'object_detection_vlm.json' - ) + default_records_file = "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 = { - label: self._empty_record() - for label in self.labels - } + self.score_records = self.get_empty_score_records() self.save_score_records() - self.get_logger().info( - f"Score records reset: {self.score_records_file}" - ) + self.get_logger().info(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( - f"Score records loaded: {self.score_records_file}" - ) + self.get_logger().info(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() - if not incoming_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}") + self.img_frame = "realsense_depth_frame" + self.get_logger().info(f"Camera info received, using frame: {self.img_frame}") - def _empty_record(self): + def get_empty_score_records(self): return { - "highest_score": 0.0, - "last_detected": None, - # 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), + 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 to current schema.""" - raw_records = {} - if os.path.exists(self.score_records_file): - with open(self.score_records_file, 'r') as f: - try: - raw_records = json.load(f) - except json.JSONDecodeError: - self.get_logger().warn("Score file corrupted, creating new one") - raw_records = {} - if not isinstance(raw_records, dict): - self.get_logger().warn("Score file format invalid, recreating records") - raw_records = {} + """Load existing score records and normalize missing labels/fields.""" + empty_records = self.get_empty_score_records() + 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 - normalized = {} - for label in self.labels: - src = raw_records.get(label, {}) - dst = self._empty_record() + with open(self.score_records_file, 'r') as f: + try: + loaded = json.load(f) + except json.JSONDecodeError: + self.get_logger().warn("Score file corrupted, creating new one") + return empty_records - highest_score = src.get("highest_score", 0.0) + 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 Exception: + except (TypeError, ValueError): highest_score = 0.0 - if not isfinite(highest_score): - highest_score = 0.0 - dst["highest_score"] = max(0.0, highest_score) - dst["last_detected"] = src.get("last_detected") + highest_score = max(0.0, highest_score) - legacy_pos = self._sanitize_position(src.get("position")) - best_pos = self._sanitize_position(src.get("best_position")) - last_pos = self._sanitize_position(src.get("last_position")) + last_detected = record.get("last_detected") + position = record.get("position") + if position is not None and not isinstance(position, dict): + position = None - if best_pos is None: - best_pos = legacy_pos - if last_pos is None: - last_pos = legacy_pos + return { + "highest_score": highest_score, + "last_detected": last_detected, + "position": position + } - dst["best_position"] = best_pos - dst["last_position"] = last_pos - dst["position"] = best_pos - normalized[label] = dst + 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 @@ -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. - - - 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. - """ + """Keep highest score per label; equal score is treated as an update.""" 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 - new_best = False - - # Always refresh timestamp when object is observed. - 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: + 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: - self.get_logger().warn( - f"Invalid map position dropped for '{label}': {position}" - ) - if record.get("position") != record.get("best_position"): - record["position"] = record.get("best_position") - changed = True + record["position"] = position + updated = 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 - is_new_record = False + # Initialize record_updated as False first + 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,76 +302,30 @@ 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) ) + + self.pose_pub.publish(ob2map) + self.get_logger().info( + f"object coordinates in map frame: x={ob2map.pose.position.x:.3f}, " + f"y={ob2map.pose.position.y:.3f}, z={ob2map.pose.position.z:.3f}" + ) + + position = { + "x": round(ob2map.pose.position.x, 4), + "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) 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( - f"object coordinates in map frame: x={ob2map.pose.position.x:.3f}, " - f"y={ob2map.pose.position.y:.3f}, z={ob2map.pose.position.z:.3f}" - ) - - position = { - "x": ob2map.pose.position.x, - "y": ob2map.pose.position.y, - "z": ob2map.pose.position.z - } - is_new_record = self.update_score_records(best_label, max_score, position=position) + 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: - log_msg += " (NEW RECORD!)" + if record_updated: + log_msg += " (BEST SCORE RECORD UPDATED)" self.get_logger().info(log_msg)