hq
/
ros2_office_vlm
1
0
Fork 2
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update

This commit is contained in:
Xu Shiyuan 2026-05-08 09:37:50 +08:00
parent 1ad9df177a
commit 987cbdabd9
2 changed files with 130 additions and 369 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
README.md
View File

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

493
src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py
View File

@ -13,9 +13,7 @@ import cv2
import json import json
import os import os
from datetime import datetime from datetime import datetime
from math import isfinite
import numpy as np import numpy as np
import yaml
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
from tf2_ros import Buffer, TransformListener from tf2_ros import Buffer, TransformListener
import tf2_geometry_msgs import tf2_geometry_msgs
@ -36,54 +34,14 @@ class ClipDetectionNode(Node):
self.bridge = CvBridge() self.bridge = CvBridge()
self.device = "cuda" if torch.cuda.is_available() else "cpu" 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 #camera init
self.latest_depth = None self.latest_depth = None
self.camera_info = None self.camera_info = None
self.latest_stamp = None self.latest_stamp = None
self.latest_rgb = None
self.latest_depth_stamp = 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) self.tf_listener = TransformListener(self.tf_buffer, self)
@ -93,39 +51,33 @@ class ClipDetectionNode(Node):
# Labels and score tracking # Labels and score tracking
self.labels = ["Refrigerator","water dispenser", "sofa", "white toilet", "office chair with wheels"] 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() # 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: try:
nav_pkg_dir = get_package_share_directory('vlm_nav_pkg') 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' nav_pkg_dir, 'config', 'example_object_detection_vlm.json'
) )
except Exception: except Exception:
self.get_logger().warn( self.get_logger().warn(
"vlm_nav_pkg not found, saving JSON to current directory" "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: 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.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: else:
self.score_records = self.load_score_records() self.score_records = self.load_score_records()
# Persist normalized schema for legacy files.
self.save_score_records() 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 # Subscribers & Publishers
self.pose_pub = self.create_publisher(PoseStamped, "/object_in_map", 10) self.pose_pub = self.create_publisher(PoseStamped, "/object_in_map", 10)
@ -150,196 +102,66 @@ class ClipDetectionNode(Node):
def camera_info_callback(self, msg): def camera_info_callback(self, msg):
self.camera_info = 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 { 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): 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):
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}"
raw_records = json.load(f) )
except json.JSONDecodeError: return empty_records
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 = {}
normalized = {} with open(self.score_records_file, 'r') as f:
for label in self.labels: try:
src = raw_records.get(label, {}) loaded = json.load(f)
dst = self._empty_record() 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: try:
highest_score = float(highest_score) highest_score = float(highest_score)
except Exception: except (TypeError, ValueError):
highest_score = 0.0 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 return normalized
@ -349,75 +171,30 @@ class ClipDetectionNode(Node):
json.dump(self.score_records, f, indent=4) json.dump(self.score_records, f, indent=4)
def update_score_records(self, label, score, position=None): 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() current_time = datetime.now().isoformat()
if label not in self.score_records: 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] 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: 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() self.save_score_records()
return new_best return updated
def rgb_callback(self, msg): def rgb_callback(self, msg):
@ -463,8 +240,8 @@ class ClipDetectionNode(Node):
if max_score >= 0.25 and best_label is not None: if max_score >= 0.25 and best_label is not None:
best_labels_per_box[i] = (best_label, max_score) 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 position = None
if self.latest_depth is not None and self.camera_info is not 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 Y = (y_center - cy) * depth / fy
Z = depth 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 = PoseStamped()
pose.header.stamp = Time().to_msg() # 零时间=使用最新可用TF pose.header.stamp = tf_stamp_msg
pose.header.frame_id = self.img_frame pose.header.frame_id = self.img_frame
pose.pose.position.x = X pose.pose.position.x = X
pose.pose.position.y = Y pose.pose.position.y = Y
@ -497,76 +302,30 @@ class ClipDetectionNode(Node):
# TF transform: camera frame -> map frame (semantic_nav 需要 map 坐标) # TF transform: camera frame -> map frame (semantic_nav 需要 map 坐标)
try: try:
ob2map = self.tf_buffer.transform( 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: except Exception as e:
err_msg = str(e) self.get_logger().error(f"TF transform to map failed: {e}")
# Fallback path: record_updated = False
# 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)
# Logging # Logging
log_msg = f"{best_label} detected with confidence score {max_score:.2f}" 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) self.get_logger().info(log_msg)