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-18 16:43:29 +08:00
parent 987cbdabd9
commit 478fb26d41
5 changed files with 507 additions and 326 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/vlm_nav_pkg/config/example_object_detection_vlm.json
View File

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

BIN
src/vlm_nav_pkg/vlm_nav_pkg/__pycache__/semantic_nav.cpython-310.pyc
View File

Binary file not shown.

429
src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py
View File

@ -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,74 +51,22 @@ class SemanticNavNode(Node):
# Initialize navigator
self.navigator = BasicNavigator()
self.navigator.waitUntilNav2Active()
self.tf_buffer = Buffer(cache_time=Duration(seconds=10))
self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True)
map_qos = QoSProfile(
depth=1,
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
)
self.map_sub = self.create_subscription(
OccupancyGrid,
"/map",
self.map_callback,
map_qos,
)
self.get_logger().info("Navigator ready")
def _load_map_bounds(self):
map_yaml = os.environ.get('NAV2_MAP_PATH', '').strip()
map_yaml = os.path.expanduser(map_yaml) if map_yaml else ''
if not map_yaml or not os.path.isfile(map_yaml):
try:
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"]
)
def query_object(self, prompt: str):
tokens = clip.tokenize([prompt]).to(self.device)
with torch.no_grad():
@ -112,46 +77,18 @@ class SemanticNavNode(Node):
best_idx = sims.argmax().item()
best_name = self.object_names[best_idx]
# Align with vlm_detection:
# prefer best_position/position, fallback to last_position when best is out-of-bounds.
# Align with vlm_detection: position may be None if not yet detected
obj_info = self.object_lib.get(best_name, {})
best_pos_raw = obj_info.get("best_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)
pos = obj_info.get("position")
if best_pos is not None and self._in_map_bounds(best_pos):
return best_name, best_pos
if best_pos is not None and not self._in_map_bounds(best_pos):
if pos is None:
self.get_logger().warn(
f"'{best_name}' best_position out of map bounds: {best_pos}"
)
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"'{best_name}' matched but has no position in JSON. "
f"Run vlm_detection in AMCL mode first!"
)
return best_name, None
self.get_logger().warn(
f"'{best_name}' has only out-of-bounds position(s), cannot navigate."
)
return best_name, None
return best_name, pos
def navigate_to_object(self, prompt: str):
name, pos = self.query_object(prompt)
@ -159,27 +96,52 @@ 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.y = pos["y"]
goal_pose.pose.position.z = pos.get("z", 0.0)
goal_pose.pose.orientation.z = 0.0
goal_pose.pose.orientation.w = 1.0
goal_pose.pose.position.x = goal_x
goal_pose.pose.position.y = goal_y
goal_pose.pose.position.z = 0.0
goal_pose.pose.orientation.z = math.sin(yaw / 2.0)
goal_pose.pose.orientation.w = math.cos(yaw / 2.0)
self.navigator.followWaypoints([goal_pose])
self.get_logger().info(f"Navigating to {name} at {pos}")
self.navigator.goToPose(deepcopy(goal_pose))
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:
@ -187,6 +149,245 @@ class SemanticNavNode(Node):
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,9 +398,13 @@ def main():
json_file_path = os.path.join(package_share_dir, 'config', 'example_object_detection_vlm.json')
node = SemanticNavNode(json_file_path)
try:
user_input = input("Where do you want to go: ")
node.navigate_to_object(user_input)
finally:
node.cleanup()
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()

BIN
src/vlm_perception_pkg/vlm_perception_pkg/__pycache__/vlm_detection.cpython-310.pyc
View File

Binary file not shown.

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

@ -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)
@ -52,31 +60,30 @@ 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.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.get_empty_score_records()
self.score_records_file = "object_detection_vlm.json"
reset_records = self.get_parameter(
'reset_records_on_startup'
).get_parameter_value().bool_value
if reset_records:
self.score_records = {
label: {"highest_score": 0.0, "last_detected": None, "position": None}
for label in self.labels
}
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.get_logger().info(f"Camera info received, using frame: {self.img_frame}")
self.latest_camera_info_stamp = msg.header.stamp
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."""
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
"""Load existing score records or create new ones"""
if os.path.exists(self.score_records_file):
with open(self.score_records_file, 'r') as f:
try:
loaded = json.load(f)
return 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
# Initialize with empty records
return {label: {"highest_score": 0.0, "last_detected": None, "position":None} for label in self.labels}
def save_score_records(self):
"""Save current score records to file"""
@ -171,27 +147,28 @@ 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,
"last_detected": None,
"position": None
"highest_score": score,
"last_detected": current_time,
"position": position
}
self.save_score_records()
return True
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 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:
record["position"] = position
self.score_records[label]["position"] = position
updated = True
record["last_detected"] = current_time
if updated:
# Always refresh timestamp (even without position/score change)
self.score_records[label]["last_detected"] = current_time
self.save_score_records()
return updated
@ -200,11 +177,10 @@ class ClipDetectionNode(Node):
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,18 +190,50 @@ 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
if self.latest_rgb is None or self.latest_depth is None or self.camera_info is None:
return
if self.latest_stamp is None or self.latest_depth_stamp is None:
return
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
with torch.no_grad():
frcnn_output = self.frcnn_model([image_tensor])[0]
candidate_boxes = frcnn_output['boxes']
candidate_scores = frcnn_output['scores']
total_candidate_count = len(candidate_boxes)
filtered_indices = [
i for i, score in enumerate(candidate_scores)
if float(score) >= frcnn_min_score
]
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)
@ -240,59 +248,29 @@ class ClipDetectionNode(Node):
if max_score >= 0.25 and best_label is not None:
best_labels_per_box[i] = (best_label, max_score)
# Initialize record_updated as False first
record_updated = False
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]
fy = self.camera_info.k[4]
cx = self.camera_info.k[2]
cy = self.camera_info.k[5]
fx = camera_info.k[0]
fy = camera_info.k[4]
cx = camera_info.k[2]
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,21 +294,16 @@ 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
# Logging
log_msg = f"{best_label} detected with confidence score {max_score:.2f}"
if record_updated:
log_msg += " (BEST SCORE RECORD UPDATED)"
if is_new_record:
log_msg += " (NEW RECORD!)"
self.get_logger().info(log_msg)
###############################################
matched_boxes = [candidate_boxes[i] for i in best_labels_per_box.keys()]
matched_labels = [f"{lbl} ({score:.2f})" for lbl, score in best_labels_per_box.values()]
@ -339,10 +312,10 @@ class ClipDetectionNode(Node):
self.annotated_pub.publish(self.bridge.cv2_to_imgmsg(annotated_img, encoding="bgr8"))
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
finally:
elapsed_ms = (time.perf_counter() - start_time) * 1000.0
self.get_logger().info(f"Detection cycle took {elapsed_ms:.1f} ms")
self.processing_frame = False
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()