diff --git a/src/vlm_nav_pkg/config/example_object_detection_vlm.json b/src/vlm_nav_pkg/config/example_object_detection_vlm.json index 2023e54..2befd64 100755 --- a/src/vlm_nav_pkg/config/example_object_detection_vlm.json +++ b/src/vlm_nav_pkg/config/example_object_detection_vlm.json @@ -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 } } } \ No newline at end of file diff --git a/src/vlm_nav_pkg/vlm_nav_pkg/__pycache__/semantic_nav.cpython-310.pyc b/src/vlm_nav_pkg/vlm_nav_pkg/__pycache__/semantic_nav.cpython-310.pyc index b5c4f83..3aafcdc 100644 Binary files a/src/vlm_nav_pkg/vlm_nav_pkg/__pycache__/semantic_nav.cpython-310.pyc and b/src/vlm_nav_pkg/vlm_nav_pkg/__pycache__/semantic_nav.cpython-310.pyc differ diff --git a/src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py b/src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py index dc07467..8bc0c5f 100755 --- a/src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py +++ b/src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py @@ -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,73 +51,21 @@ class SemanticNavNode(Node): # Initialize navigator self.navigator = BasicNavigator() self.navigator.waitUntilNav2Active() - 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"] + 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 query_object(self, prompt: str): tokens = clip.tokenize([prompt]).to(self.device) @@ -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,35 +96,299 @@ 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: self.get_logger().info("Navigation canceled") 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,10 +398,14 @@ def main(): json_file_path = os.path.join(package_share_dir, 'config', 'example_object_detection_vlm.json') node = SemanticNavNode(json_file_path) - user_input = input("Where do you want to go: ") - node.navigate_to_object(user_input) - - rclpy.shutdown() + 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() if __name__ == "__main__": diff --git a/src/vlm_perception_pkg/vlm_perception_pkg/__pycache__/vlm_detection.cpython-310.pyc b/src/vlm_perception_pkg/vlm_perception_pkg/__pycache__/vlm_detection.cpython-310.pyc index 3485a9f..bf68c90 100644 Binary files a/src/vlm_perception_pkg/vlm_perception_pkg/__pycache__/vlm_detection.cpython-310.pyc and b/src/vlm_perception_pkg/vlm_perception_pkg/__pycache__/vlm_detection.cpython-310.pyc differ 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 20dc727..f8998dc 100755 --- a/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py +++ b/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py @@ -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) @@ -51,32 +59,31 @@ 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 - - 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 - - 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 + """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: + return json.load(f) + except json.JSONDecodeError: + self.get_logger().warn("Score file corrupted, creating new one") + + # 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,40 +147,40 @@ 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 } - - 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 position is not None: - record["position"] = position - updated = True - - record["last_detected"] = current_time - if updated: self.save_score_records() - + return True + + updated = False + 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: + self.score_records[label]["position"] = position + updated = True + + # Always refresh timestamp (even without position/score change) + self.score_records[label]["last_detected"] = current_time + self.save_score_records() + return updated 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,85 +190,87 @@ 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 - cv_img = self.latest_rgb.copy() - image_tensor = transforms.ToTensor()(cv_img) + if self.latest_rgb is None or self.latest_depth is None or self.camera_info is None: + return - with torch.no_grad(): - frcnn_output = self.frcnn_model([image_tensor])[0] - candidate_boxes = frcnn_output['boxes'] + if self.latest_stamp is None or self.latest_depth_stamp is None: + return - best_labels_per_box = {} # key: box index, value: (label, score) + 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 - for i, box in enumerate(candidate_boxes): - max_score = -1 - best_label = None - for label in self.labels: - _, score = self.match_label_box(label, image_tensor, [box]) - if score > max_score: - max_score = score - best_label = label + 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) - 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 - position = None + 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) + + for i, box in enumerate(candidate_boxes): + max_score = -1 + best_label = None + for label in self.labels: + _, score = self.match_label_box(label, image_tensor, [box]) + if score > max_score: + max_score = score + best_label = label + + if max_score >= 0.25 and best_label is not None: + best_labels_per_box[i] = (best_label, max_score) + 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,33 +294,28 @@ 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 + log_msg = f"{best_label} detected with confidence score {max_score:.2f}" + if is_new_record: + log_msg += " (NEW RECORD!)" + self.get_logger().info(log_msg) - # Logging - log_msg = f"{best_label} detected with confidence score {max_score:.2f}" - if record_updated: - log_msg += " (BEST SCORE RECORD UPDATED)" - 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()] - -############################################### - - 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()] - - annotated_img = self.draw_boxes(cv_img, matched_boxes, matched_labels) - try: - 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 + annotated_img = self.draw_boxes(cv_img, matched_boxes, matched_labels) + try: + 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}") + 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()