fengyujia
/
ros2_office_vlm
forked from hq/ros2_office_vlm
1
0
Fork 0
Code Pull Requests 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 格式的语义记忆库到指定路径。
```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

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

@ -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)