diff --git a/Asjad/perception/launch/demo_webcam_pose.launch.py b/Asjad/perception/launch/demo_webcam_pose.launch.py deleted file mode 100644 index afaf247..0000000 --- a/Asjad/perception/launch/demo_webcam_pose.launch.py +++ /dev/null @@ -1,68 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument( - 'model_path', - default_value='models/tools.pt', - description='Path to the YOLO model file' - ), - - DeclareLaunchArgument( - 'device_id', - default_value='0', - description='Camera Device ID (0 for webcam, 1 for external, etc.)' - ), - - # 1. Webcam Publisher - Node( - package='perception', - executable='opencv_camera_node', - name='webcam_publisher', - output='screen', - parameters=[{ - 'topic_name': '/camera/color/image_raw', - 'device_id': LaunchConfiguration('device_id') - }] - ), - - # 2. Mock Depth Publisher - Node( - package='perception', - executable='mock_depth_node', - name='mock_depth_publisher', - output='screen' - ), - - # 3. YOLO Node - Node( - package='perception', - executable='yolo_node', - name='yolo_detector', - output='screen', - parameters=[{ - 'model_path': LaunchConfiguration('model_path'), - 'input_mode': 'robot' # Consumes standard topics - }], - remappings=[ - ('/detections', '/tool_detector/detections') - ] - ), - - # 4. Pose Node - Node( - package='perception', - executable='pose_node', - name='pose_estimator', - output='screen', - parameters=[{ - 'input_mode': 'robot' # Consumes standard topics - }], - remappings=[ - ('/detections', '/tool_detector/detections') - ] - ) - ]) diff --git a/Asjad/perception/launch/tool_pipeline.launch.py b/Asjad/perception/launch/tool_pipeline.launch.py deleted file mode 100644 index 82bf7bd..0000000 --- a/Asjad/perception/launch/tool_pipeline.launch.py +++ /dev/null @@ -1,43 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument( - 'model_path', - default_value='models/tools.pt', - description='Path to the YOLO model file' - ), - DeclareLaunchArgument( - 'input_mode', - default_value='realsense', - description='Input mode: robot, realsense, or topic name' - ), - Node( - package='perception', - executable='yolo_node', - name='yolo_detector', - output='screen', - parameters=[{ - 'model_path': LaunchConfiguration('model_path'), - 'input_mode': LaunchConfiguration('input_mode') - }], - remappings=[ - ('/detections', '/tool_detector/detections') - ] - ), - Node( - package='perception', - executable='pose_node', - name='pose_estimator', - output='screen', - parameters=[{ - 'input_mode': LaunchConfiguration('input_mode') - }], - remappings=[ - ('/detections', '/tool_detector/detections') - ] - ) - ]) diff --git a/Asjad/perception/package.xml b/Asjad/perception/package.xml deleted file mode 100644 index 39c5db7..0000000 --- a/Asjad/perception/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - perception - 0.0.0 - TODO: Package description - mohsin - TODO: License declaration - - sensor_msgs - std_msgs - rclpy - image_transport - cv_bridge - python3-opencv - opencv4 - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/Asjad/perception/perception/mock_depth_publisher.py b/Asjad/perception/perception/mock_depth_publisher.py deleted file mode 100644 index e80964e..0000000 --- a/Asjad/perception/perception/mock_depth_publisher.py +++ /dev/null @@ -1,56 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image, PointCloud2, PointField -from cv_bridge import CvBridge -import numpy as np -import struct -import sensor_msgs_py.point_cloud2 as pc2 - -class MockDepthPublisher(Node): - def __init__(self): - super().__init__('mock_depth_publisher') - self.bridge = CvBridge() - - # Subscribe to webcam image - self.create_subscription(Image, '/camera/color/image_raw', self.image_callback, 10) - - # Publish mock pointcloud - self.pc_pub = self.create_publisher(PointCloud2, '/camera/depth/color/points', 10) - - self.get_logger().info("Mock Depth Publisher Started. Generating planar depth at Z=1.0m") - - def image_callback(self, img_msg): - # Create a simple planar pointcloud matching the image resolution - height = img_msg.height - width = img_msg.width - - # Generate grid of X, Y coordinates - # Simplified intrinsic model: assume center is (w/2, h/2) and focal length ~ width - fx, fy = width, width - cx, cy = width / 2, height / 2 - - u, v = np.meshgrid(np.arange(width), np.arange(height)) - z = np.ones_like(u) * 1.0 # Fixed depth of 1 meter - - x = (u - cx) * z / fx - y = (v - cy) * z / fy - - # Stack into (N, 3) array - points = np.stack([x.flatten(), y.flatten(), z.flatten()], axis=1) - - # Create header - header = img_msg.header - - # Create PointCloud2 - pc_msg = pc2.create_cloud_xyz32(header, points) - self.pc_pub.publish(pc_msg) - -def main(args=None): - rclpy.init(args=args) - node = MockDepthPublisher() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/Asjad/perception/perception/opencv_camera_feed.py b/Asjad/perception/perception/opencv_camera_feed.py deleted file mode 100644 index 0502e9d..0000000 --- a/Asjad/perception/perception/opencv_camera_feed.py +++ /dev/null @@ -1,63 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image -from cv_bridge import CvBridge -import cv2 -import numpy as np - -class SimpleCameraNode(Node): - def __init__(self): - super().__init__('simple_camera_node') - - # Declare parameters - self.declare_parameter('device_id', 0) - self.declare_parameter('topic_name', '/camera/color/image_raw') - - device_id = self.get_parameter('device_id').get_parameter_value().integer_value - topic_name = self.get_parameter('topic_name').get_parameter_value().string_value - - self.publisher = self.create_publisher(Image, topic_name, 10) - self.bridge = CvBridge() - - self.get_logger().info(f"Opening camera device {device_id}...") - self.cap = cv2.VideoCapture(device_id) - - self.use_synthetic = False - if not self.cap.isOpened(): - self.get_logger().warn(f"Could not open camera device {device_id}! Switching to SYNTHETIC MODE (Test Pattern).") - self.use_synthetic = True - self.synthetic_frame = np.zeros((480, 640, 3), dtype=np.uint8) - # Draw a "tool" (green rectangle) to detect - cv2.rectangle(self.synthetic_frame, (200, 150), (440, 330), (0, 255, 0), -1) - - timer_period = 0.03 # 30ms ~ 33 FPS - self.timer = self.create_timer(timer_period, self.timer_callback) - - def timer_callback(self): - if self.use_synthetic: - frame = self.synthetic_frame.copy() - # Add some noise or movement? - pass - else: - ret, frame = self.cap.read() - if not ret: - # self.get_logger().warn('Failed to capture frame') - return - - msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') - msg.header.stamp = self.get_clock().now().to_msg() # Important for synchronization - msg.header.frame_id = "camera_frame" - self.publisher.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - node = SimpleCameraNode() - rclpy.spin(node) - node.cap.release() - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/Asjad/perception/perception/pose_pca.py b/Asjad/perception/perception/pose_pca.py deleted file mode 100644 index 73c9a8a..0000000 --- a/Asjad/perception/perception/pose_pca.py +++ /dev/null @@ -1,216 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import PointCloud2, PointField, Image -from vision_msgs.msg import Detection2DArray -from geometry_msgs.msg import PoseStamped, TransformStamped -from std_msgs.msg import Header - -import message_filters -from cv_bridge import CvBridge -import numpy as np -import struct -import sensor_msgs_py.point_cloud2 as pc2 -from transforms3d.quaternions import mat2quat -from tf2_ros import TransformBroadcaster - - -class PointCloudCropperNode(Node): - def __init__(self): - super().__init__('pointcloud_cropper_node') - - self.bridge = CvBridge() - - # Declare input_mode parameter only - self.declare_parameter('input_mode', 'realsense') - input_mode = self.get_parameter('input_mode').get_parameter_value().string_value - - # Determine topics based on mode - if input_mode == 'robot': - pc_topic = '/camera/depth/color/points' - img_topic = '/camera/color/image_raw' - elif input_mode == 'realsense': - pc_topic = '/camera/camera/depth/color/points' - img_topic = '/camera/camera/color/image_raw' - else: - self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") - pc_topic = '/camera/camera/depth/color/points' - img_topic = '/camera/camera/color/image_raw' - - self.get_logger().info(f"Using input mode: '{input_mode}' with topics: {pc_topic}, {img_topic}") - - # Message filter subscribers - pc_sub = message_filters.Subscriber(self, PointCloud2, pc_topic) - img_sub = message_filters.Subscriber(self, Image, img_topic) - det_sub = message_filters.Subscriber(self, Detection2DArray, '/detections') - - ts = message_filters.ApproximateTimeSynchronizer( - [pc_sub, det_sub, img_sub], - queue_size=10, - slop=0.1 - ) - ts.registerCallback(self.sync_callback) - - self.prev_poses = {} # Store previous pose for smoothing per object index - self.alpha_pos = 0.2 # low-pass filter factor for position (0.2 = slow/smooth, 1.0 = no smoothing) - self.alpha_rot = 0.1 # low-pass filter factor for rotation - - self.get_logger().info('PointCloud Cropper Node with robust PCA starting...') - - def sync_callback(self, cloud_msg, detection_msg, image_msg): - try: - color_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8') - except Exception as e: - self.get_logger().error(f"Image conversion error: {e}") - return - - pc_width = cloud_msg.width - pc_height = cloud_msg.height - - # Read all points (expensive but robust) - cloud_points = np.array([ - [x, y, z] - for x, y, z in pc2.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=False) - ]).reshape((pc_height, pc_width, 3)) - - all_colored_points = [] - current_frame_poses = {} - - for idx, detection in enumerate(detection_msg.detections): - detected_class = detection.results[0].hypothesis.class_id - - # 1. Extract ROI - cx, cy = int(detection.bbox.center.position.x), int(detection.bbox.center.position.y) - w, h = int(detection.bbox.size_x), int(detection.bbox.size_y) - xmin, xmax = max(cx - w // 2, 0), min(cx + w // 2, pc_width) - ymin, ymax = max(cy - h // 2, 0), min(cy + h // 2, pc_height) - - cropped_points = cloud_points[ymin:ymax, xmin:xmax, :].reshape(-1, 3) - cropped_colors = color_image[ymin:ymax, xmin:xmax, :].reshape(-1, 3) - - # Basic NaN filtering - valid_mask = ~np.isnan(cropped_points).any(axis=1) - cropped_points = cropped_points[valid_mask] - cropped_colors = cropped_colors[valid_mask] - - # 2. Outlier Removal (Z-Score) - if len(cropped_points) > 10: - mean = np.mean(cropped_points, axis=0) - std = np.std(cropped_points, axis=0) - # Filter points > 2 std devs away - z_score = np.abs((cropped_points - mean) / (std + 1e-6)) - inlier_mask = (z_score < 2.0).all(axis=1) - cropped_points = cropped_points[inlier_mask] - cropped_colors = cropped_colors[inlier_mask] - - # Re-collect for visualization - for pt, color in zip(cropped_points, cropped_colors): - x, y, z = pt - rgb = struct.unpack('f', struct.pack('I', (int(color[2]) << 16) | (int(color[1]) << 8) | int(color[0])))[0] - all_colored_points.append([x, y, z, rgb]) - - if len(cropped_points) < 10: - continue - - # 3. PCA & Alignment - centroid = np.mean(cropped_points, axis=0) - centered = cropped_points - centroid - _, _, vh = np.linalg.svd(centered, full_matrices=False) - R = vh.T - - # 3a. Normal Alignment (Minor Axis Z should point to Camera) - # Camera frame: Z is forward (positive). - # Objects are in front, so Z > 0. Normal pointing "at" camera means Z component should be negative. - # However, if we view surface, normal is usually out of surface. - # Let's enforce Z component of Z-axis (R[2,2]) is negative (pointing back to origin). - if R[2, 2] > 0: - R[:, 2] *= -1 - R[:, 1] *= -1 # Maintain right-hand rule by flipping Y too - - # 3b. Direction Disambiguation (Major Axis X towards "heavy" side) - # Project points onto X axis - projected_x = centered @ R[:, 0] - # Simple heuristic: Skewness. If skew > 0, tail is right, bulk is left. - # We want X to point to bulk. So if skew > 0, flip X. - skew = np.sum(projected_x ** 3) - if skew > 0: - R[:, 0] *= -1 - R[:, 1] *= -1 # Maintain RHR - - # 4. Temporal Smoothing - prev = self.prev_poses.get(idx) - - if prev is not None: - # Position Smoothing - centroid = self.alpha_pos * centroid + (1 - self.alpha_pos) * prev['pos'] - - # Rotation Smoothing (Quaternion Lerp) - q_curr = mat2quat(R) - q_prev = prev['quat'] - - # Ensure shortest path (dot product > 0) - dot = np.dot(q_curr, q_prev) - if dot < 0: - q_curr = -q_curr - - q_smooth = self.alpha_rot * q_curr + (1 - self.alpha_rot) * q_prev - q_smooth /= np.linalg.norm(q_smooth) # Normalize - quat = q_smooth - - # Reconstruct R from smoothed quat for other uses if needed (omitted for speed) - else: - quat = mat2quat(R) - - # Store for next frame - current_frame_poses[idx] = {'pos': centroid, 'quat': quat} - - # Publish - pose_msg = PoseStamped() - pose_msg.header.stamp = self.get_clock().now().to_msg() - pose_msg.header.frame_id = cloud_msg.header.frame_id - pose_msg.pose.position.x = float(centroid[0]) - pose_msg.pose.position.y = float(centroid[1]) - pose_msg.pose.position.z = float(centroid[2]) - pose_msg.pose.orientation.x = float(quat[1]) # Transforms3d gives w,x,y,z. ROS needs x,y,z,w - pose_msg.pose.orientation.y = float(quat[2]) - pose_msg.pose.orientation.z = float(quat[3]) - pose_msg.pose.orientation.w = float(quat[0]) - - self.pose_pub.publish(pose_msg) - - # TF - t = TransformStamped() - t.header = pose_msg.header - t.child_frame_id = f'object_frame_{idx}' - t.transform.translation.x = pose_msg.pose.position.x - t.transform.translation.y = pose_msg.pose.position.y - t.transform.translation.z = pose_msg.pose.position.z - t.transform.rotation = pose_msg.pose.orientation - self.tf_broadcaster.sendTransform(t) - - self.prev_poses = current_frame_poses # Update state with only currently seen objects (avoids stale ghosts) - - if all_colored_points: - # ... (Publish cropped cloud code matches existing structure) - header = Header() - header.stamp = self.get_clock().now().to_msg() - header.frame_id = cloud_msg.header.frame_id - fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), - PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), - ] - cropped_pc = pc2.create_cloud(header, fields, all_colored_points) - self.pc_pub.publish(cropped_pc) - - -def main(args=None): - rclpy.init(args=args) - node = PointCloudCropperNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/Asjad/perception/perception/yolo_object_detection.py b/Asjad/perception/perception/yolo_object_detection.py deleted file mode 100644 index c59fa7b..0000000 --- a/Asjad/perception/perception/yolo_object_detection.py +++ /dev/null @@ -1,130 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image -from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose -from cv_bridge import CvBridge -from ultralytics import YOLO -from ultralytics.engine.results import Boxes -import torch -import cv2 -import os - -# Monkeypatch torch.load to disable weights_only enforcement by default -# Required for Ultralytics 8.1.0 with PyTorch 2.6+ -if hasattr(torch, 'load'): - original_load = torch.load - def safe_load(*args, **kwargs): - if 'weights_only' not in kwargs: - kwargs['weights_only'] = False - return original_load(*args, **kwargs) - torch.load = safe_load - - -class YoloDetectorNode(Node): - def __init__(self): - super().__init__('yolo_detector_node') - self.bridge = CvBridge() - - # Declare and get parameters - self.declare_parameter('model_path', '') - self.declare_parameter('model_type', 'default') # 'default' or 'fine_tuned' (deprecated if model_path used) - self.declare_parameter('input_mode', 'realsense') # 'robot' or 'realsense' - - model_path_param = self.get_parameter('model_path').get_parameter_value().string_value - model_type = self.get_parameter('model_type').get_parameter_value().string_value - input_mode = self.get_parameter('input_mode').get_parameter_value().string_value - - # Determine model path - if model_path_param: - model_path = model_path_param - elif model_type == 'fine_tuned': - # Fallback for legacy support, but user should prefer model_path - model_path = os.path.join(os.getcwd(), 'ros2_ws', 'models', 'fine_tuned.pt') - else: - model_path = 'yolov8n.pt' # Ultralytics will download/find this - - self.get_logger().info(f"Using model from: {model_path}") - self.model = YOLO(model_path) - - # Determine image topic - if input_mode == 'robot': - image_topic = '/camera/color/image_raw' - elif input_mode == 'realsense': - image_topic = '/camera/camera/color/image_raw' - else: - # Check if input_mode is actually a topic name - if input_mode.startswith('/'): - image_topic = input_mode - else: - self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") - image_topic = '/camera/camera/color/image_raw' - - self.get_logger().info(f"Subscribing to image topic: {image_topic}") - - # Create subscriptions and publishers - self.image_sub = self.create_subscription(Image, image_topic, self.image_callback, 10) - self.annotated_image_pub = self.create_publisher(Image, '/annotated_image', 10) - self.detection_pub = self.create_publisher(Detection2DArray, '/detections', 10) - - self.conf_threshold = 0.6 # Confidence threshold for filtering - - self.get_logger().info('YOLOv8 Detector Node started.') - - def image_callback(self, msg): - try: - cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') - except Exception as e: - self.get_logger().error(f'Image conversion error: {e}') - return - - results = self.model(cv_image) - detection_array_msg = Detection2DArray() - detection_array_msg.header = msg.header - - for result in results: - filtered_boxes = [box for box in result.boxes if float(box.conf) >= self.conf_threshold] - - if filtered_boxes: - box_data = torch.stack([b.data[0] for b in filtered_boxes]) - result.boxes = Boxes(box_data, orig_shape=result.orig_shape) - else: - result.boxes = Boxes(torch.empty((0, 6)), orig_shape=result.orig_shape) - - annotated_image = result.plot() - try: - annotated_msg = self.bridge.cv2_to_imgmsg(annotated_image, encoding='bgr8') - annotated_msg.header = msg.header - self.annotated_image_pub.publish(annotated_msg) - except Exception as e: - self.get_logger().error(f'Annotated image conversion error: {e}') - - for box in filtered_boxes: - detection_msg = Detection2D() - detection_msg.header = msg.header - - hypothesis = ObjectHypothesisWithPose() - hypothesis.hypothesis.class_id = self.model.names[int(box.cls)] - hypothesis.hypothesis.score = float(box.conf) - detection_msg.results.append(hypothesis) - - xywh = box.xywh.cpu().numpy().flatten() - detection_msg.bbox.center.position.x = float(xywh[0]) - detection_msg.bbox.center.position.y = float(xywh[1]) - detection_msg.bbox.size_x = float(xywh[2]) - detection_msg.bbox.size_y = float(xywh[3]) - - detection_array_msg.detections.append(detection_msg) - - self.detection_pub.publish(detection_array_msg) - - -def main(args=None): - rclpy.init(args=args) - node = YoloDetectorNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/Asjad/perception/setup.py b/Asjad/perception/setup.py deleted file mode 100644 index f537b6d..0000000 --- a/Asjad/perception/setup.py +++ /dev/null @@ -1,33 +0,0 @@ -from setuptools import find_packages, setup -from glob import glob -import os - -package_name = 'perception' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='mohsin', - maintainer_email='', - description='TODO: Package description', - license='Apache-2.0', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - "yolo_node = perception.yolo_object_detection:main", - "pose_node = perception.pose_pca:main", - "opencv_camera_node = perception.opencv_camera_feed:main", - "opencv_yolo = perception.opencv_yolo_object_detection:main", - "mock_depth_node = perception.mock_depth_publisher:main", - ], - }, -) diff --git a/Asjad/scripts/demos/test_realsense_windows.py b/Asjad/scripts/demos/test_realsense_windows.py deleted file mode 100644 index 85927f1..0000000 --- a/Asjad/scripts/demos/test_realsense_windows.py +++ /dev/null @@ -1,65 +0,0 @@ -import pyrealsense2 as rs -import time - -def main(): - print("---------------------------------------") - print("RealSense Windows Diagnostics") - print("---------------------------------------") - - ctx = rs.context() - devices = ctx.query_devices() - - if len(devices) == 0: - print("ERROR: No RealSense devices found!") - print("Check your USB connection.") - return - - for dev in devices: - print(f"Device Found: {dev.get_info(rs.camera_info.name)}") - print(f" Serial: {dev.get_info(rs.camera_info.serial_number)}") - print(f" Firmware: {dev.get_info(rs.camera_info.firmware_version)}") - - # Check USB Type - try: - usb_type = dev.get_info(rs.camera_info.usb_type_descriptor) - print(f" USB Connection: {usb_type}") - if "2." in usb_type: - print(" [WARNING] Connected via USB 2.0! Streaming capabilities will be limited.") - print(" Use a high-quality USB 3.0 (Type-C to Type-A/C) cable.") - except: - print(" USB Connection: Unknown") - - print("\nAttempting to start stream...") - pipeline = rs.pipeline() - config = rs.config() - - # Try a modest resolution that works on USB 2.0 and 3.0 - config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) - config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) - - try: - profile = pipeline.start(config) - print("SUCCESS: Stream started!") - - # Check active profile - active_depth = profile.get_stream(rs.stream.depth) - print(f"Active Stream: {active_depth.as_video_stream_profile().width()}x{active_depth.as_video_stream_profile().height()} @ {active_depth.as_video_stream_profile().fps()}fps") - - print("Streaming 50 frames...") - for i in range(50): - frames = pipeline.wait_for_frames() - if i % 10 == 0: - print(f" Frame {i}/50 received...") - - pipeline.stop() - print("SUCCESS: Test Complete. Hardware is working.") - print("---------------------------------------") - - except Exception as e: - print(f"\n[ERROR] Failed to stream: {e}") - print("Possible causes:") - print("1. USB Bandwidth (Try a different port/cable)") - print("2. Camera is in a bad state (Unplug/Replug)") - -if __name__ == "__main__": - main() diff --git a/Asjad/scripts/demos/webcam_demo.py b/Asjad/scripts/demos/webcam_demo.py deleted file mode 100644 index 0aa023f..0000000 --- a/Asjad/scripts/demos/webcam_demo.py +++ /dev/null @@ -1,36 +0,0 @@ -import torch -import cv2 -from ultralytics import YOLO - -# Monkeypatch torch.load to disable weights_only enforcement -# This is necessary because we are using PyTorch 2.6+ with Ultralytics 8.1.0 -if hasattr(torch, 'load'): - original_load = torch.load - def safe_load(*args, **kwargs): - if 'weights_only' not in kwargs: - kwargs['weights_only'] = False - return original_load(*args, **kwargs) - torch.load = safe_load - -def main(): - # Load the trained model - model_path = 'models/tools.pt' - print(f"Loading model from {model_path}...") - try: - model = YOLO(model_path) - except Exception as e: - print(f"Error loading model: {e}") - return - - # Run inference on the webcam (source=0) - # show=True opens a window with results - # conf=0.6 sets confidence threshold - print("Starting webcam... Press 'q' to exit (if window allows) or Ctrl+C in terminal.") - results = model.predict(source='0', show=True, conf=0.6, stream=True) - - # We iterate over the stream to keep the script running - for r in results: - pass # The show=True handles the display - -if __name__ == '__main__': - main() diff --git a/Asjad/scripts/training/train_tool_model.py b/Asjad/scripts/training/train_tool_model.py deleted file mode 100644 index f6bb9b6..0000000 --- a/Asjad/scripts/training/train_tool_model.py +++ /dev/null @@ -1,26 +0,0 @@ -import torch -import os - -# Monkeypatch torch.load to disable weights_only enforcement by default -# This is necessary because PyTorch 2.6+ defaults weights_only=True, -# but Ultralytics 8.1.0 (legacy req) relies on the old behavior for loading yolov8s.pt -original_load = torch.load -def safe_load(*args, **kwargs): - if 'weights_only' not in kwargs: - kwargs['weights_only'] = False - return original_load(*args, **kwargs) -torch.load = safe_load - -from ultralytics import YOLO - -def main(): - print("Starting YOLOv8 training with patched torch.load...") - # Load model - model = YOLO('yolov8s.pt') - - # Train - # data path must be correct relative to cwd - model.train(data='datasets/tools/data.yaml', epochs=50, imgsz=640) - -if __name__ == '__main__': - main() diff --git a/Feature_Readme/Table_Height_Readme/README.md b/Feature_Readme/Table_Height_Readme/README.md index 8ccb6b8..d6f1c8d 100644 --- a/Feature_Readme/Table_Height_Readme/README.md +++ b/Feature_Readme/Table_Height_Readme/README.md @@ -1,6 +1,6 @@ ### Running Table Height -```code -ros2 run table_height_predictor table_heigt +``` +ros2 run perception table_height ``` ### Running Floor Detection ``` @@ -19,4 +19,3 @@ desk. - It has an error rate of +-5cm to the actual height so it should be used with a safety margin. In the example above the actual height was 58cm. - This can work with varying heights of the table. - diff --git a/README.md b/README.md index 81b0966..dc21f5c 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,6 @@ # Release_3 Feature Readmes - [Table_Segmentation](https://github.com/Robots4Sustainability/perception/tree/dev/Feature_Readme/Table_Segmentation_Readme) - [Table_Height](https://github.com/Robots4Sustainability/perception/blob/dev/Feature_Readme/Table_Height_Readme/README.md) -- [Action_Server](https://github.com/Robots4Sustainability/perception/blob/dev/Feature_Readme/Action_Server_Readme/README.md) - [Tool_Detection](https://github.com/Robots4Sustainability/perception/blob/dev/Feature_Readme/Tool_Detection/README.md) --- @@ -20,6 +19,7 @@ This repository provides a complete vision pipeline that supports object detecti - [Project Structure](#project-structure) - [Pose Estimation Setup](#pose-estimation-setup) - [Launching the Camera and Robot](#launching-the-camera-and-robot) +- [Action Pipeline](#action-server) - [Running the Nodes](#running-the-nodes) - [Option 1: All-in-One Launch (Camera + Nodes)](#1-both-camera-and-the-nodes) - [Option 2: Nodes-Only Launch](#2-all-the-nodes-via-one-launch-file) @@ -110,19 +110,59 @@ export RMW_IMPLEMENTATION=rmw_zenoh_cpp your kinova vision node ``` +To connect the workstation topics to another local PC/Laptop +```bash +# Workstation Terminal +ros2 run rmw_zenoh_cpp rmw_zenohd + + +# Local PC Terminal +export ZENOH_CONFIG_OVERRIDE='connect/endpoints=["tcp/192.168.1.11:7447"]' +ros2 run rmw_zenoh_cpp rmw_zenohd + +``` + Command to run kinova_vision.lanch ```bash -ros2 launch kinova_vision kinova_vision.launch.py \ - device:=192.168.1.12 \ - depth_registration:=true \ - color_camera_info_url:=package://kinova_vision/launch/calibration/default_color_calib_1280x720.ini +ros2 launch kinova_vision kinova_vision.launch.py device:=192.168.1.12 depth_registration:=true color_camera_info_url:=file:///home/mohsin/r4s/src/ros2_kortex_vision/launch/calibration/default_color_calib_1280x720.ini depth_camera_info_url:=file:///home/mohsin/r4s/src/ros2_kortex_vision/launch/calibration/default_depth_calib_480x270.ini ``` If you get resolution errors then, go to the admin portal of the arm(192.168.1.1X) and then in the Camera settings, set the calibration to 1280x720 --- +## Action Server + +```bash +# Terminal 1 +ros2 launch perception action_launch.py + +# Terminal 2 +ros2 run perception brain_client +``` +- table_height +- detect_screws +- car_objects +- subdoor_pose +- detect_screwdriver +- place_object + +To Debug/Check Status +```bash +# All Nodes + for node in $(ros2 lifecycle nodes); do echo -n "$node: "; ros2 lifecycle get $node; done + + # Each Node + ros2 lifecycle set /subdoor_node activate + ros2 lifecycle get /subdoor_node +``` +- configure +- activate +- deactivate + +--- + ## Running the Nodes The project provides multiple ways to launch the files. diff --git a/no_place.png b/no_place.png deleted file mode 100644 index d643b10..0000000 Binary files a/no_place.png and /dev/null differ diff --git a/place_object_1.png b/place_object_1.png deleted file mode 100644 index 8e6e390..0000000 Binary files a/place_object_1.png and /dev/null differ diff --git a/place_object_2.png b/place_object_2.png deleted file mode 100644 index 6ad8aa4..0000000 Binary files a/place_object_2.png and /dev/null differ diff --git a/r4s/output3.log b/r4s/output3.log new file mode 100644 index 0000000..f2eb56c --- /dev/null +++ b/r4s/output3.log @@ -0,0 +1,373 @@ +[INFO] [1774868862.425055742] [table_height_estimator]: Table Height Lifecycle Node Initialized (Unconfigured). +[INFO] [1774868922.101718896] [table_height_estimator]: Configuring: lightweight geometric mode (no heavy models) +[INFO] [1774868922.155207080] [table_height_estimator]: Configuration complete. +[INFO] [1774868922.155436563] [table_height_estimator]: returned 1 +[INFO] [1774868922.321218033] [table_height_estimator]: returned 3 +[INFO] [1774868922.416592110] [table_height_estimator]: returned 3 +[INFO] [1774868922.725491482] [table_height_estimator]: returned 3 +[INFO] [1774868922.823558266] [table_height_estimator]: returned 3 +[INFO] [1774868923.006772351] [table_height_estimator]: returned 3 +[INFO] [1774868923.184538748] [table_height_estimator]: returned 3 +[INFO] [1774868923.299673018] [table_height_estimator]: Activating node... +[INFO] [1774868923.300665511] [table_height_estimator]: Node activated. Processing incoming data. +[INFO] [1774868923.503580619] [table_height_estimator]: returned 10 +[INFO] [1774868923.671246137] [table_height_estimator]: returned 10 +[INFO] [1774868923.842391600] [table_height_estimator]: returned 10 +[INFO] [1774868923.999979001] [table_height_estimator]: returned 10 +[INFO] [1774868924.142042421] [table_height_estimator]: returned 10 +[INFO] [1774868924.311018655] [table_height_estimator]: returned 10 +[INFO] [1774868924.476488379] [table_height_estimator]: returned 10 +[INFO] [1774868924.628938725] [table_height_estimator]: returned 10 +[INFO] [1774868924.831470655] [table_height_estimator]: returned 10 +[INFO] [1774868924.987593172] [table_height_estimator]: returned 10 +[INFO] [1774868925.217818564] [table_height_estimator]: returned 10 +[INFO] [1774868925.428688446] [table_height_estimator]: returned 10 +[INFO] [1774868925.599055824] [table_height_estimator]: returned 10 +[INFO] [1774868925.760935829] [table_height_estimator]: returned 10 +[INFO] [1774868925.957602127] [table_height_estimator]: returned 10 +[INFO] [1774868926.187705462] [table_height_estimator]: returned 10 +[INFO] [1774868926.350504839] [table_height_estimator]: returned 10 +[INFO] [1774868926.510730951] [table_height_estimator]: returned 10 +[INFO] [1774868926.670164511] [table_height_estimator]: returned 10 +[INFO] [1774868926.830742475] [table_height_estimator]: returned 10 +[INFO] [1774868926.996587014] [table_height_estimator]: returned 10 +[INFO] [1774868927.184812622] [table_height_estimator]: returned 10 +[INFO] [1774868927.333873197] [table_height_estimator]: returned 10 +[INFO] [1774868927.659743211] [table_height_estimator]: returned 10 +[INFO] [1774868927.841045961] [table_height_estimator]: returned 10 +[INFO] [1774868928.005415825] [table_height_estimator]: returned 10 +[INFO] [1774868928.170013837] [table_height_estimator]: returned 10 +[INFO] [1774868928.892147106] [table_height_estimator]: returned 10 +[INFO] [1774868929.054924798] [table_height_estimator]: returned 10 +[INFO] [1774868929.230559388] [table_height_estimator]: returned 10 +[INFO] [1774868929.482921093] [table_height_estimator]: returned 10 +[INFO] [1774868929.775582372] [table_height_estimator]: returned 10 +[INFO] [1774868930.025525167] [table_height_estimator]: returned 10 +[INFO] [1774868930.212046716] [table_height_estimator]: returned 10 +[INFO] [1774868930.392521719] [table_height_estimator]: returned 10 +[INFO] [1774868930.563999122] [table_height_estimator]: returned 10 +[INFO] [1774868930.703624308] [table_height_estimator]: returned 10 +[INFO] [1774868930.845565110] [table_height_estimator]: returned 10 +[INFO] [1774868930.954672098] [table_height_estimator]: returned 10 +[INFO] [1774868931.063747082] [table_height_estimator]: returned 10 +[INFO] [1774868931.164863504] [table_height_estimator]: returned 10 +[INFO] [1774868931.288185428] [table_height_estimator]: returned 10 +[INFO] [1774868931.464829592] [table_height_estimator]: returned 10 +[INFO] [1774868931.636791301] [table_height_estimator]: returned 10 +[INFO] [1774868931.870362770] [table_height_estimator]: returned 10 +[INFO] [1774868932.082955755] [table_height_estimator]: returned 10 +[INFO] [1774868932.253878617] [table_height_estimator]: returned 10 +[INFO] [1774868932.419302434] [table_height_estimator]: returned 10 +[INFO] [1774868932.617021398] [table_height_estimator]: returned 10 +[INFO] [1774868932.834349911] [table_height_estimator]: returned 10 +[INFO] [1774868933.020293293] [table_height_estimator]: returned 10 +[INFO] [1774868933.120207446] [table_height_estimator]: returned 10 +[INFO] [1774868933.272455104] [table_height_estimator]: returned 10 +[INFO] [1774868933.449348324] [table_height_estimator]: returned 10 +[INFO] [1774868933.645208162] [table_height_estimator]: returned 10 +[INFO] [1774868933.795256228] [table_height_estimator]: returned 10 +[INFO] [1774868933.954035989] [table_height_estimator]: returned 10 +[INFO] [1774868934.302023447] [table_height_estimator]: returned 10 +[INFO] [1774868934.479575688] [table_height_estimator]: returned 10 +[INFO] [1774868934.669509596] [table_height_estimator]: returned 10 +[INFO] [1774868934.839095103] [table_height_estimator]: returned 10 +[INFO] [1774868935.523261562] [table_height_estimator]: returned 10 +[INFO] [1774868935.709720389] [table_height_estimator]: returned 10 +[INFO] [1774868935.853871410] [table_height_estimator]: returned 10 +[INFO] [1774868936.071224469] [table_height_estimator]: returned 10 +[INFO] [1774868936.280647592] [table_height_estimator]: returned 10 +[INFO] [1774868936.444367973] [table_height_estimator]: returned 10 +[INFO] [1774868936.628546312] [table_height_estimator]: returned 10 +[INFO] [1774868936.806911999] [table_height_estimator]: returned 10 +[INFO] [1774868936.981233352] [table_height_estimator]: returned 10 +[INFO] [1774868937.148989244] [table_height_estimator]: returned 10 +[INFO] [1774868937.307844344] [table_height_estimator]: returned 10 +[INFO] [1774868937.482964167] [table_height_estimator]: returned 10 +[INFO] [1774868937.663586587] [table_height_estimator]: returned 10 +[INFO] [1774868937.823635710] [table_height_estimator]: returned 10 +[INFO] [1774868937.983585107] [table_height_estimator]: returned 10 +[INFO] [1774868938.164552588] [table_height_estimator]: returned 10 +[INFO] [1774868938.327415754] [table_height_estimator]: returned 10 +[INFO] [1774868938.578062097] [table_height_estimator]: returned 10 +[INFO] [1774868938.771718181] [table_height_estimator]: returned 10 +[INFO] [1774868938.936128679] [table_height_estimator]: returned 10 +[INFO] [1774868939.112112420] [table_height_estimator]: returned 10 +[INFO] [1774868939.305979936] [table_height_estimator]: returned 10 +[INFO] [1774868939.529085846] [table_height_estimator]: returned 10 +[INFO] [1774868939.708000731] [table_height_estimator]: returned 10 +[INFO] [1774868939.806397738] [table_height_estimator]: returned 10 +[INFO] [1774868939.954299192] [table_height_estimator]: returned 10 +[INFO] [1774868940.157439858] [table_height_estimator]: returned 10 +[INFO] [1774868940.310301642] [table_height_estimator]: returned 10 +[INFO] [1774868940.485115747] [table_height_estimator]: returned 10 +[INFO] [1774868940.664900714] [table_height_estimator]: returned 10 +[INFO] [1774868940.998964677] [table_height_estimator]: returned 10 +[INFO] [1774868941.167366263] [table_height_estimator]: returned 10 +[INFO] [1774868941.351578889] [table_height_estimator]: returned 10 +[INFO] [1774868941.511340142] [table_height_estimator]: returned 10 +[INFO] [1774868942.219456042] [table_height_estimator]: returned 10 +[INFO] [1774868942.386995377] [table_height_estimator]: returned 10 +[INFO] [1774868942.538818604] [table_height_estimator]: returned 10 +[INFO] [1774868942.759974724] [table_height_estimator]: returned 10 +[INFO] [1774868942.957060038] [table_height_estimator]: returned 10 +[INFO] [1774868943.141303853] [table_height_estimator]: returned 10 +[INFO] [1774868943.332640343] [table_height_estimator]: returned 10 +[INFO] [1774868943.481535782] [table_height_estimator]: returned 10 +[INFO] [1774868943.655387067] [table_height_estimator]: returned 10 +[INFO] [1774868943.813191460] [table_height_estimator]: returned 10 +[INFO] [1774868943.980267623] [table_height_estimator]: returned 10 +[INFO] [1774868944.171090987] [table_height_estimator]: returned 10 +[INFO] [1774868944.342032471] [table_height_estimator]: returned 10 +[INFO] [1774868944.529346030] [table_height_estimator]: returned 10 +[INFO] [1774868944.677831329] [table_height_estimator]: returned 10 +[INFO] [1774868944.842823560] [table_height_estimator]: returned 10 +[INFO] [1774868945.012111274] [table_height_estimator]: returned 10 +[INFO] [1774868945.248539918] [table_height_estimator]: returned 10 +[INFO] [1774868945.451472781] [table_height_estimator]: returned 10 +[INFO] [1774868945.624968766] [table_height_estimator]: returned 10 +[INFO] [1774868945.828316810] [table_height_estimator]: returned 10 +[INFO] [1774868946.017007068] [table_height_estimator]: returned 10 +[INFO] [1774868946.235974587] [table_height_estimator]: returned 10 +[INFO] [1774868946.409640693] [table_height_estimator]: returned 10 +[INFO] [1774868946.535298739] [table_height_estimator]: returned 10 +[INFO] [1774868946.653890589] [table_height_estimator]: returned 10 +[INFO] [1774868946.825256113] [table_height_estimator]: returned 10 +[INFO] [1774868947.013122434] [table_height_estimator]: returned 10 +[INFO] [1774868947.171824309] [table_height_estimator]: returned 10 +[INFO] [1774868947.329828068] [table_height_estimator]: returned 10 +[INFO] [1774868947.684497022] [table_height_estimator]: returned 10 +[INFO] [1774868947.859342582] [table_height_estimator]: returned 10 +[INFO] [1774868948.021362491] [table_height_estimator]: returned 10 +[INFO] [1774868948.211791968] [table_height_estimator]: returned 10 +[INFO] [1774868948.909900651] [table_height_estimator]: returned 10 +[INFO] [1774868949.096322817] [table_height_estimator]: returned 10 +[INFO] [1774868949.235716145] [table_height_estimator]: returned 10 +[INFO] [1774868949.453965911] [table_height_estimator]: returned 10 +[INFO] [1774868949.654429660] [table_height_estimator]: returned 10 +[INFO] [1774868949.842461290] [table_height_estimator]: returned 10 +[INFO] [1774868949.993194077] [table_height_estimator]: returned 10 +[INFO] [1774868950.173191120] [table_height_estimator]: returned 10 +[INFO] [1774868950.349843209] [table_height_estimator]: returned 10 +[INFO] [1774868950.513765373] [table_height_estimator]: returned 10 +[INFO] [1774868950.671974071] [table_height_estimator]: returned 10 +[INFO] [1774868950.851344356] [table_height_estimator]: returned 10 +[INFO] [1774868951.023544366] [table_height_estimator]: returned 10 +[INFO] [1774868951.231543495] [table_height_estimator]: returned 10 +[INFO] [1774868951.357155471] [table_height_estimator]: returned 10 +[INFO] [1774868951.545019689] [table_height_estimator]: returned 10 +[INFO] [1774868951.708472304] [table_height_estimator]: returned 10 +[INFO] [1774868951.961929219] [table_height_estimator]: returned 10 +[INFO] [1774868952.161125249] [table_height_estimator]: returned 10 +[INFO] [1774868952.322985693] [table_height_estimator]: returned 10 +[INFO] [1774868952.495266594] [table_height_estimator]: returned 10 +[INFO] [1774868952.693125444] [table_height_estimator]: returned 10 +[INFO] [1774868952.909225400] [table_height_estimator]: returned 10 +[INFO] [1774868953.095609896] [table_height_estimator]: returned 10 +[INFO] [1774868953.197096929] [table_height_estimator]: returned 10 +[INFO] [1774868953.336104868] [table_height_estimator]: returned 10 +[INFO] [1774868953.528188302] [table_height_estimator]: returned 10 +[INFO] [1774868953.699399297] [table_height_estimator]: returned 10 +[INFO] [1774868953.873130600] [table_height_estimator]: returned 10 +[INFO] [1774868954.032082887] [table_height_estimator]: returned 10 +[INFO] [1774868954.394809704] [table_height_estimator]: returned 10 +[INFO] [1774868954.547378922] [table_height_estimator]: returned 10 +[INFO] [1774868954.752925397] [table_height_estimator]: returned 10 +[INFO] [1774868954.937823277] [table_height_estimator]: returned 10 +[INFO] [1774868955.719293360] [table_height_estimator]: returned 10 +[INFO] [1774868955.882227702] [table_height_estimator]: returned 10 +[INFO] [1774868956.020377898] [table_height_estimator]: returned 10 +[INFO] [1774868956.159299635] [table_height_estimator]: returned 10 +[INFO] [1774868956.354403310] [table_height_estimator]: returned 10 +[INFO] [1774868956.515066738] [table_height_estimator]: returned 10 +[INFO] [1774868956.694798957] [table_height_estimator]: returned 10 +[INFO] [1774868956.859857986] [table_height_estimator]: returned 10 +[INFO] [1774868957.035943582] [table_height_estimator]: returned 10 +[INFO] [1774868957.216295511] [table_height_estimator]: returned 10 +[INFO] [1774868957.355375703] [table_height_estimator]: returned 10 +[INFO] [1774868957.533643906] [table_height_estimator]: returned 10 +[INFO] [1774868957.721933524] [table_height_estimator]: returned 10 +[INFO] [1774868957.899624301] [table_height_estimator]: returned 10 +[INFO] [1774868958.044581545] [table_height_estimator]: returned 10 +[INFO] [1774868958.234269032] [table_height_estimator]: returned 10 +[INFO] [1774868958.395579527] [table_height_estimator]: returned 10 +[INFO] [1774868958.648416556] [table_height_estimator]: returned 10 +[INFO] [1774868958.867971730] [table_height_estimator]: returned 10 +[INFO] [1774868959.003404229] [table_height_estimator]: returned 10 +[INFO] [1774868959.202987698] [table_height_estimator]: returned 10 +[INFO] [1774868959.377823206] [table_height_estimator]: returned 10 +[INFO] [1774868959.599631600] [table_height_estimator]: returned 10 +[INFO] [1774868959.755401721] [table_height_estimator]: returned 10 +[INFO] [1774868959.862731362] [table_height_estimator]: returned 10 +[INFO] [1774868960.035850192] [table_height_estimator]: returned 10 +[INFO] [1774868960.225804815] [table_height_estimator]: returned 10 +[INFO] [1774868960.393501822] [table_height_estimator]: returned 10 +[INFO] [1774868960.577587217] [table_height_estimator]: returned 10 +[INFO] [1774868960.730617498] [table_height_estimator]: returned 10 +[INFO] [1774868961.077048212] [table_height_estimator]: returned 10 +[INFO] [1774868961.249829156] [table_height_estimator]: returned 10 +[INFO] [1774868961.408724281] [table_height_estimator]: returned 10 +[INFO] [1774868961.594418086] [table_height_estimator]: returned 10 +[INFO] [1774868962.293900269] [table_height_estimator]: returned 10 +[INFO] [1774868962.465942890] [table_height_estimator]: returned 10 +[INFO] [1774868962.642780642] [table_height_estimator]: returned 10 +[INFO] [1774868962.851165736] [table_height_estimator]: returned 10 +[INFO] [1774868963.072086696] [table_height_estimator]: returned 10 +[INFO] [1774868963.224733252] [table_height_estimator]: returned 10 +[INFO] [1774868963.404552986] [table_height_estimator]: returned 10 +[INFO] [1774868963.558393847] [table_height_estimator]: returned 10 +[INFO] [1774868963.731490158] [table_height_estimator]: returned 10 +[INFO] [1774868963.906273781] [table_height_estimator]: returned 10 +[INFO] [1774868964.055181007] [table_height_estimator]: returned 10 +[INFO] [1774868964.234253741] [table_height_estimator]: returned 10 +[INFO] [1774868964.415239678] [table_height_estimator]: returned 10 +[INFO] [1774868964.599869455] [table_height_estimator]: returned 10 +[INFO] [1774868964.744090056] [table_height_estimator]: returned 10 +[INFO] [1774868964.924875735] [table_height_estimator]: returned 10 +[INFO] [1774868965.104937913] [table_height_estimator]: returned 10 +[INFO] [1774868965.325568882] [table_height_estimator]: returned 10 +[INFO] [1774868965.523166111] [table_height_estimator]: returned 10 +[INFO] [1774868965.708241626] [table_height_estimator]: returned 10 +[INFO] [1774868965.865581109] [table_height_estimator]: returned 10 +[INFO] [1774868966.068615020] [table_height_estimator]: returned 10 +[INFO] [1774868966.289471910] [table_height_estimator]: returned 10 +[INFO] [1774868966.469290030] [table_height_estimator]: returned 10 +[INFO] [1774868966.573100445] [table_height_estimator]: returned 10 +[INFO] [1774868966.738612597] [table_height_estimator]: returned 10 +[INFO] [1774868966.913077093] [table_height_estimator]: returned 10 +[INFO] [1774868967.082003596] [table_height_estimator]: returned 10 +[INFO] [1774868967.251784342] [table_height_estimator]: returned 10 +[INFO] [1774868967.415422070] [table_height_estimator]: returned 10 +[INFO] [1774868967.784189302] [table_height_estimator]: returned 10 +[INFO] [1774868967.940745710] [table_height_estimator]: returned 10 +[INFO] [1774868968.102735184] [table_height_estimator]: returned 10 +[INFO] [1774868968.283171450] [table_height_estimator]: returned 10 +[INFO] [1774868968.985153453] [table_height_estimator]: returned 10 +[INFO] [1774868969.183679372] [table_height_estimator]: returned 10 +[INFO] [1774868969.318631434] [table_height_estimator]: returned 10 +[INFO] [1774868969.550227887] [table_height_estimator]: returned 10 +[INFO] [1774868969.749599160] [table_height_estimator]: returned 10 +[INFO] [1774868969.917035305] [table_height_estimator]: returned 10 +[INFO] [1774868970.094074882] [table_height_estimator]: returned 10 +[INFO] [1774868970.268999839] [table_height_estimator]: returned 10 +[INFO] [1774868970.442216305] [table_height_estimator]: returned 10 +[INFO] [1774868970.598872167] [table_height_estimator]: returned 10 +[INFO] [1774868970.742339549] [table_height_estimator]: returned 10 +[INFO] [1774868970.929213297] [table_height_estimator]: returned 10 +[INFO] [1774868971.114527672] [table_height_estimator]: returned 10 +[INFO] [1774868971.293709215] [table_height_estimator]: returned 10 +[INFO] [1774868971.465206863] [table_height_estimator]: returned 10 +[INFO] [1774868971.640246561] [table_height_estimator]: returned 10 +[INFO] [1774868971.802941836] [table_height_estimator]: returned 10 +[INFO] [1774868972.055009733] [table_height_estimator]: returned 10 +[INFO] [1774868972.257481848] [table_height_estimator]: returned 10 +[INFO] [1774868972.409856215] [table_height_estimator]: returned 10 +[INFO] [1774868972.563111897] [table_height_estimator]: returned 10 +[INFO] [1774868972.781204831] [table_height_estimator]: returned 10 +[INFO] [1774868972.988703879] [table_height_estimator]: returned 10 +[INFO] [1774868973.155332919] [table_height_estimator]: returned 10 +[INFO] [1774868973.264478624] [table_height_estimator]: returned 10 +[INFO] [1774868973.432855807] [table_height_estimator]: returned 10 +[INFO] [1774868973.624495033] [table_height_estimator]: returned 10 +[INFO] [1774868973.770329202] [table_height_estimator]: returned 10 +[INFO] [1774868973.947561384] [table_height_estimator]: returned 10 +[INFO] [1774868974.110239310] [table_height_estimator]: returned 10 +[INFO] [1774868974.464843644] [table_height_estimator]: returned 10 +[INFO] [1774868974.627311334] [table_height_estimator]: returned 10 +[INFO] [1774868974.810007327] [table_height_estimator]: returned 10 +[INFO] [1774868974.984940095] [table_height_estimator]: returned 10 +[INFO] [1774868975.706649883] [table_height_estimator]: returned 10 +[INFO] [1774868975.879040835] [table_height_estimator]: returned 10 +[INFO] [1774868976.017301422] [table_height_estimator]: returned 10 +[INFO] [1774868976.245112555] [table_height_estimator]: returned 10 +[INFO] [1774868976.447123000] [table_height_estimator]: returned 10 +[INFO] [1774868976.613496787] [table_height_estimator]: returned 10 +[INFO] [1774868976.788795717] [table_height_estimator]: returned 10 +[INFO] [1774868976.960035538] [table_height_estimator]: returned 10 +[INFO] [1774868977.135099412] [table_height_estimator]: returned 10 +[INFO] [1774868977.291382279] [table_height_estimator]: returned 10 +[INFO] [1774868977.449049139] [table_height_estimator]: returned 10 +[INFO] [1774868977.634768521] [table_height_estimator]: returned 10 +[INFO] [1774868977.820013530] [table_height_estimator]: returned 10 +[INFO] [1774868977.988580118] [table_height_estimator]: returned 10 +[INFO] [1774868978.151242625] [table_height_estimator]: returned 10 +[INFO] [1774868978.324226221] [table_height_estimator]: returned 10 +[INFO] [1774868978.502865962] [table_height_estimator]: returned 10 +[INFO] [1774868978.727994039] [table_height_estimator]: returned 10 +[INFO] [1774868978.930994231] [table_height_estimator]: returned 10 +[INFO] [1774868979.099286046] [table_height_estimator]: returned 10 +[INFO] [1774868979.404575514] [table_height_estimator]: returned 10 +[INFO] [1774868979.649807602] [table_height_estimator]: returned 10 +[INFO] [1774868979.804429804] [table_height_estimator]: returned 10 +[INFO] [1774868979.956839801] [table_height_estimator]: returned 10 +[INFO] [1774868980.099115263] [table_height_estimator]: returned 10 +[INFO] [1774868980.205323349] [table_height_estimator]: returned 10 +[INFO] [1774868980.317688673] [table_height_estimator]: returned 10 +[INFO] [1774868980.496957649] [table_height_estimator]: returned 10 +[INFO] [1774868980.662548231] [table_height_estimator]: returned 10 +[INFO] [1774868980.818606498] [table_height_estimator]: returned 10 +[INFO] [1774868981.168316002] [table_height_estimator]: returned 10 +[INFO] [1774868981.333185866] [table_height_estimator]: returned 10 +[INFO] [1774868981.525295809] [table_height_estimator]: returned 10 +[INFO] [1774868981.717337933] [table_height_estimator]: returned 10 +[INFO] [1774868982.380406324] [table_height_estimator]: returned 10 +[INFO] [1774868982.561605539] [table_height_estimator]: returned 10 +[INFO] [1774868982.716485516] [table_height_estimator]: returned 10 +[INFO] [1774868982.924302581] [table_height_estimator]: returned 10 +[INFO] [1774868983.129339939] [table_height_estimator]: returned 10 +[INFO] [1774868983.307259639] [table_height_estimator]: returned 10 +[INFO] [1774868983.469484291] [table_height_estimator]: returned 10 +[INFO] [1774868983.648738899] [table_height_estimator]: returned 10 +[INFO] [1774868983.818281276] [table_height_estimator]: returned 10 +[INFO] [1774868983.980180269] [table_height_estimator]: returned 10 +[INFO] [1774868984.138675727] [table_height_estimator]: returned 10 +[INFO] [1774868984.320925417] [table_height_estimator]: returned 10 +[INFO] [1774868984.504466630] [table_height_estimator]: returned 10 +[INFO] [1774868984.685255604] [table_height_estimator]: returned 10 +[INFO] [1774868984.830863663] [table_height_estimator]: returned 10 +[INFO] [1774868985.021399957] [table_height_estimator]: returned 10 +[INFO] [1774868985.195864037] [table_height_estimator]: returned 10 +[INFO] [1774868985.427106513] [table_height_estimator]: returned 10 +[INFO] [1774868985.626701841] [table_height_estimator]: returned 10 +[INFO] [1774868985.807276345] [table_height_estimator]: returned 10 +[INFO] [1774868985.959595623] [table_height_estimator]: returned 10 +[INFO] [1774868986.186403807] [table_height_estimator]: returned 10 +[INFO] [1774868986.373927261] [table_height_estimator]: returned 10 +[INFO] [1774868986.550523603] [table_height_estimator]: returned 10 +[INFO] [1774868986.658139121] [table_height_estimator]: returned 10 +[INFO] [1774868986.829583839] [table_height_estimator]: returned 10 +[INFO] [1774868986.999713452] [table_height_estimator]: returned 10 +[INFO] [1774868987.166271645] [table_height_estimator]: returned 10 +[INFO] [1774868987.342518291] [table_height_estimator]: returned 10 +[INFO] [1774868987.506687410] [table_height_estimator]: returned 10 +Traceback (most recent call last): + File "/home/afnan/Desktop/R4S table height/perception/r4s/install/perception/lib/python3.12/site-packages/perception/table_height_node.py", line 265, in main + rclpy.spin(node) + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 247, in spin + executor.spin_once() + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 845, in spin_once + self._spin_once_impl(timeout_sec) + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 829, in _spin_once_impl + handler, entity, node = self.wait_for_ready_callbacks( + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 799, in wait_for_ready_callbacks + return next(self._cb_iter) + ^^^^^^^^^^^^^^^^^^^ + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/executors.py", line 707, in _wait_for_ready_callbacks + raise ExternalShutdownException() +rclpy.executors.ExternalShutdownException + +During handling of the above exception, another exception occurred: + +Traceback (most recent call last): + File "/home/afnan/Desktop/R4S table height/perception/r4s/install/perception/lib/perception/table_height", line 33, in + sys.exit(load_entry_point('perception==0.0.0', 'console_scripts', 'table_height')()) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + File "/home/afnan/Desktop/R4S table height/perception/r4s/install/perception/lib/python3.12/site-packages/perception/table_height_node.py", line 270, in main + rclpy.shutdown() + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown + _shutdown(context=context) + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown + context.shutdown() + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown + self.__context.shutdown() +rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 diff --git a/r4s/output4.log b/r4s/output4.log new file mode 100644 index 0000000..b116285 --- /dev/null +++ b/r4s/output4.log @@ -0,0 +1,15 @@ +[INFO] [1774869005.593187110] [table_height_estimator]: Table Height Lifecycle Node Initialized (Unconfigured). +Traceback (most recent call last): + File "/home/afnan/Desktop/R4S table height/perception/r4s/install/perception/lib/perception/table_height", line 33, in + sys.exit(load_entry_point('perception==0.0.0', 'console_scripts', 'table_height')()) + ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + File "/home/afnan/Desktop/R4S table height/perception/r4s/install/perception/lib/python3.12/site-packages/perception/table_height_node.py", line 270, in main + rclpy.shutdown() + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/__init__.py", line 134, in shutdown + _shutdown(context=context) + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/utilities.py", line 82, in shutdown + context.shutdown() + File "/opt/ros/jazzy/lib/python3.12/site-packages/rclpy/context.py", line 129, in shutdown + self.__context.shutdown() +rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called on the given context, at ./src/rcl/init.c:333 +[ros2run]: Process exited with failure 1 diff --git a/src/my_robot_interfaces/CMakeLists.txt b/r4s/src/my_robot_interfaces/CMakeLists.txt similarity index 100% rename from src/my_robot_interfaces/CMakeLists.txt rename to r4s/src/my_robot_interfaces/CMakeLists.txt diff --git a/r4s/src/my_robot_interfaces/action/RunVision.action b/r4s/src/my_robot_interfaces/action/RunVision.action new file mode 100644 index 0000000..46ad65e --- /dev/null +++ b/r4s/src/my_robot_interfaces/action/RunVision.action @@ -0,0 +1,13 @@ +# --- GOAL --- +string task_name # e.g., 'car_objects', 'table_height', 'screwdriver', 'subdoor', 'screws' +float32 time_duration +--- +# --- RESULT --- +bool success +string message +geometry_msgs/PoseStamped[] poses # Array: Can hold 1 pose (Screwdriver), 4 poses (Subdoor), or many +float32 estimated_value # Used for the Table Height (Z-axis offset) +--- +# --- FEEDBACK --- +string current_phase # e.g., "Waking up YOLO-OBB", "Averaging Poses", "Calculating Height" +float32 time_elapsed \ No newline at end of file diff --git a/src/my_robot_interfaces/package.xml b/r4s/src/my_robot_interfaces/package.xml similarity index 100% rename from src/my_robot_interfaces/package.xml rename to r4s/src/my_robot_interfaces/package.xml diff --git a/r4s/src/perception/launch/action_launch.py b/r4s/src/perception/launch/action_launch.py new file mode 100644 index 0000000..b1b3e5b --- /dev/null +++ b/r4s/src/perception/launch/action_launch.py @@ -0,0 +1,166 @@ +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, LifecycleNode + +def generate_launch_description(): + # --- 1. Launch Arguments --- + input_mode_arg = DeclareLaunchArgument( + 'input_mode', + default_value='robot', + description="Camera input mode: 'robot' or 'realsense'" + ) + + input_mode = LaunchConfiguration('input_mode') + pkg_name = 'perception' + + # --- 2. Lifecycle Nodes --- + + # ========================================== + # TABLE HEIGHT + # ========================================== + table_height_node = LifecycleNode( + package=pkg_name, + executable='table_height', + name='table_height_estimator', + namespace='', + output='screen' + ) + + # ========================================== + # SUBDOOR POSE + # ========================================== + subdoor_node = LifecycleNode( + package=pkg_name, + executable='subdoor_node', + name='subdoor_pose_estimator', + namespace='', + output='screen', + parameters=[{'input_mode': input_mode}] + ) + + # ========================================== + # Place Object (Table Segmentation) + # ========================================== + + place_object_node = LifecycleNode( + package=pkg_name, + executable='table_segmentation_node', + name='place_object_node', + namespace='', + output='screen', + parameters=[{'mode': 'test', 'input_mode': input_mode}] + ) + + # ========================================== + # PIPELINE 1: SCREWS (YOLO) + # ========================================== + yolo_screw_node = LifecycleNode( + package=pkg_name, + executable='action_yolo_node', + name='yolo_screw_node', + namespace='', + parameters=[{'model_type': 'screw', 'input_mode': input_mode}], + remappings=[ + ('/yolo/detections', '/screw/detections') + ] + ) + + cropper_screw_node = LifecycleNode( + package=pkg_name, + executable='action_pose_node', + name='cropper_screw_node', + namespace='', + parameters=[{'input_mode': input_mode}], + remappings=[ + ('/yolo/detections', '/screw/detections'), + ('/object_pose', '/screw/pose'), + ('/perception/detected_object_radius', '/screw/radius') + ] + ) + + # ========================================== + # PIPELINE 2: CAR OBJECTS (YOLO) + # ========================================== + yolo_car_node = LifecycleNode( + package=pkg_name, + executable='action_yolo_node', + name='yolo_car_node', + namespace='', + parameters=[{'model_type': 'fine_tuned', 'input_mode': input_mode}], + remappings=[ + ('/yolo/detections', '/car/detections') + ] + ) + + cropper_car_node = LifecycleNode( + package=pkg_name, + executable='action_pose_node', + name='cropper_car_node', + namespace='', + parameters=[{'input_mode': input_mode}], + remappings=[ + ('/yolo/detections', '/car/detections'), + ('/object_pose', '/car/pose'), + ('/perception/detected_object_radius', '/car/radius') + ] + ) + + # ========================================== + # PIPELINE 3: SCREWDRIVER (OBB) + # ========================================== + obb_detector_node = LifecycleNode( + package=pkg_name, + executable='obb_node', + name='obb_detector_node', + namespace='', + output='screen', + parameters=[{ + 'model_type': 'fine_tuned', + 'input_mode': input_mode + }] + ) + + obb_cropper_node = LifecycleNode( + package=pkg_name, + executable='pose_obb_node', + name='point_obb_cloud_cropper_node', + namespace='', + output='screen', + parameters=[{'input_mode': input_mode}] + ) + + # --- 3. Standard Nodes --- + dispatcher_node = Node( + package=pkg_name, + executable='vision_manager_node', + name='perception_pipeline_node', + output='screen' + ) + + # --- 4. Build Launch Description --- + + ld = LaunchDescription() + + # Add Arguments + ld.add_action(input_mode_arg) + + # Add Lifecycle Nodes + ld.add_action(table_height_node) + ld.add_action(subdoor_node) + ld.add_action(place_object_node) + + ld.add_action(yolo_screw_node) + ld.add_action(cropper_screw_node) + + ld.add_action(yolo_car_node) + ld.add_action(cropper_car_node) + + ld.add_action(obb_detector_node) + ld.add_action(obb_cropper_node) + + # Add Dispatcher last so the lifecycle nodes exist before it tries to manage them + ld.add_action(dispatcher_node) + + return ld \ No newline at end of file diff --git a/src/perception/launch/camera_and_perception_launch.py b/r4s/src/perception/launch/camera_and_perception_launch.py similarity index 100% rename from src/perception/launch/camera_and_perception_launch.py rename to r4s/src/perception/launch/camera_and_perception_launch.py diff --git a/src/perception/launch/camera_launch.py b/r4s/src/perception/launch/camera_launch.py similarity index 100% rename from src/perception/launch/camera_launch.py rename to r4s/src/perception/launch/camera_launch.py diff --git a/src/perception/launch/perception_launch.py b/r4s/src/perception/launch/perception_launch.py similarity index 100% rename from src/perception/launch/perception_launch.py rename to r4s/src/perception/launch/perception_launch.py diff --git a/src/perception/models/fine_tuned.pt b/r4s/src/perception/models/fine_tuned.pt similarity index 100% rename from src/perception/models/fine_tuned.pt rename to r4s/src/perception/models/fine_tuned.pt diff --git a/src/perception/models/readme.md b/r4s/src/perception/models/readme.md similarity index 100% rename from src/perception/models/readme.md rename to r4s/src/perception/models/readme.md diff --git a/r4s/src/perception/models/screw_best.pt b/r4s/src/perception/models/screw_best.pt new file mode 100644 index 0000000..afc2673 Binary files /dev/null and b/r4s/src/perception/models/screw_best.pt differ diff --git a/r4s/src/perception/models/screwdriver_obb_best.pt b/r4s/src/perception/models/screwdriver_obb_best.pt new file mode 100644 index 0000000..ca73e0e Binary files /dev/null and b/r4s/src/perception/models/screwdriver_obb_best.pt differ diff --git a/r4s/src/perception/models/subdoor.pt b/r4s/src/perception/models/subdoor.pt new file mode 100644 index 0000000..94c3a6c Binary files /dev/null and b/r4s/src/perception/models/subdoor.pt differ diff --git a/src/perception/models/yolov8n.pt b/r4s/src/perception/models/yolov8n.pt similarity index 100% rename from src/perception/models/yolov8n.pt rename to r4s/src/perception/models/yolov8n.pt diff --git a/src/perception/package.xml b/r4s/src/perception/package.xml similarity index 90% rename from src/perception/package.xml rename to r4s/src/perception/package.xml index 2193750..2503d3d 100644 --- a/src/perception/package.xml +++ b/r4s/src/perception/package.xml @@ -14,6 +14,8 @@ cv_bridge python3-opencv opencv4 + python3-sklearn + python3-open3d ament_copyright ament_flake8 diff --git a/r4s/src/perception/perception.egg-info/PKG-INFO b/r4s/src/perception/perception.egg-info/PKG-INFO new file mode 100644 index 0000000..2c9f0e3 --- /dev/null +++ b/r4s/src/perception/perception.egg-info/PKG-INFO @@ -0,0 +1,17 @@ +Metadata-Version: 2.4 +Name: perception +Version: 0.0.0 +Summary: TODO: Package description +Maintainer: mohsin +Maintainer-email: mohsinalimirxa@gmail.com +License: TODO: License declaration +Requires-Dist: setuptools +Requires-Dist: open3d +Requires-Dist: scikit-learn +Requires-Dist: numpy +Requires-Dist: opencv-python +Dynamic: license +Dynamic: maintainer +Dynamic: maintainer-email +Dynamic: requires-dist +Dynamic: summary diff --git a/r4s/src/perception/perception.egg-info/SOURCES.txt b/r4s/src/perception/perception.egg-info/SOURCES.txt new file mode 100644 index 0000000..0231b97 --- /dev/null +++ b/r4s/src/perception/perception.egg-info/SOURCES.txt @@ -0,0 +1,37 @@ +package.xml +setup.cfg +setup.py +launch/action_launch.py +launch/camera_and_perception_launch.py +launch/camera_launch.py +launch/perception_launch.py +models/fine_tuned.pt +models/readme.md +models/screw_best.pt +models/screwdriver_obb_best.pt +models/subdoor.pt +models/yolov8n.pt +perception/__init__.py +perception/action_obb_object_detection.py +perception/action_obb_pose.py +perception/action_pose_pca.py +perception/action_vision_manager.py +perception/action_yolo_object_detection.py +perception/brain_client.py +perception/subdoor.py +perception/subdoor_pose_estimator.py +perception/table_height_node.py +perception/table_segmentation.py +perception/yolo_object_detection.py +perception/yolo_pose.py +perception.egg-info/PKG-INFO +perception.egg-info/SOURCES.txt +perception.egg-info/dependency_links.txt +perception.egg-info/entry_points.txt +perception.egg-info/requires.txt +perception.egg-info/top_level.txt +perception.egg-info/zip-safe +resource/perception +test/test_copyright.py +test/test_flake8.py +test/test_pep257.py \ No newline at end of file diff --git a/Asjad/ignore.html b/r4s/src/perception/perception.egg-info/dependency_links.txt similarity index 100% rename from Asjad/ignore.html rename to r4s/src/perception/perception.egg-info/dependency_links.txt diff --git a/r4s/src/perception/perception.egg-info/entry_points.txt b/r4s/src/perception/perception.egg-info/entry_points.txt new file mode 100644 index 0000000..39434d6 --- /dev/null +++ b/r4s/src/perception/perception.egg-info/entry_points.txt @@ -0,0 +1,13 @@ +[console_scripts] +action_pose_node = perception.action_pose_pca:main +action_yolo_node = perception.action_yolo_object_detection:main +brain_client = perception.brain_client:main +obb_node = perception.action_obb_object_detection:main +pose_node = perception.yolo_pose:main +pose_obb_node = perception.action_obb_pose:main +subdoor = perception.subdoor:main +subdoor_node = perception.subdoor_pose_estimator:main +table_height = perception.table_height_node:main +table_segmentation_node = perception.table_segmentation:main +vision_manager_node = perception.action_vision_manager:main +yolo_node = perception.yolo_object_detection:main diff --git a/r4s/src/perception/perception.egg-info/requires.txt b/r4s/src/perception/perception.egg-info/requires.txt new file mode 100644 index 0000000..90bd8d2 --- /dev/null +++ b/r4s/src/perception/perception.egg-info/requires.txt @@ -0,0 +1,5 @@ +setuptools +open3d +scikit-learn +numpy +opencv-python diff --git a/r4s/src/perception/perception.egg-info/top_level.txt b/r4s/src/perception/perception.egg-info/top_level.txt new file mode 100644 index 0000000..52cd4e4 --- /dev/null +++ b/r4s/src/perception/perception.egg-info/top_level.txt @@ -0,0 +1 @@ +perception diff --git a/r4s/src/perception/perception.egg-info/zip-safe b/r4s/src/perception/perception.egg-info/zip-safe new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/r4s/src/perception/perception.egg-info/zip-safe @@ -0,0 +1 @@ + diff --git a/Asjad/perception/perception/__init__.py b/r4s/src/perception/perception/__init__.py similarity index 100% rename from Asjad/perception/perception/__init__.py rename to r4s/src/perception/perception/__init__.py diff --git a/r4s/src/perception/perception/action_obb_object_detection.py b/r4s/src/perception/perception/action_obb_object_detection.py new file mode 100644 index 0000000..2129265 --- /dev/null +++ b/r4s/src/perception/perception/action_obb_object_detection.py @@ -0,0 +1,213 @@ +import rclpy +from rclpy.lifecycle import Node as LifecycleNode, State, TransitionCallbackReturn +from sensor_msgs.msg import Image +from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose +from cv_bridge import CvBridge +from ultralytics import YOLO +import torch +import cv2 +import os +from ament_index_python.packages import get_package_share_directory +from pathlib import Path + +def get_package_name_from_path(file_path): + """Dynamically find the package name from the file path.""" + p = Path(file_path) + package_parts = p.parts[p.parts.index('site-packages') + 1:] + return package_parts[0] + +class OBBDetectorLifecycleNode(LifecycleNode): + def __init__(self): + self.package_name = get_package_name_from_path(__file__) + super().__init__('obb_detector_node') + self.bridge = CvBridge() + + # Declare parameters + self.declare_parameter('model_type', 'fine_tuned') + self.declare_parameter('input_mode', 'robot') + self.declare_parameter('model_path', '') + self.declare_parameter('conf_threshold', 0.6) + self.declare_parameter('device', '') + self.declare_parameter('class_names', []) + + # Placeholders for resources + self.model = None + self.class_names = None + self.image_sub = None + self.annotated_image_pub = None + self.detection_pub = None + self.conf_threshold = 0.6 + + self._is_active = False + self.get_logger().info('YOLOv8-OBB Detector Lifecycle Node Initialized (Unconfigured).') + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Configuring OBB Detector Node...") + + try: + model_type = self.get_parameter('model_type').get_parameter_value().string_value + input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + explicit_model_path = self.get_parameter('model_path').get_parameter_value().string_value + self.conf_threshold = self.get_parameter('conf_threshold').get_parameter_value().double_value + device_param = self.get_parameter('device').get_parameter_value().string_value + class_names_param = self.get_parameter('class_names').get_parameter_value().string_array_value + + # Determine model path + if explicit_model_path: + model_path = explicit_model_path + else: + package_share_directory = get_package_share_directory(self.package_name) + if model_type == 'fine_tuned': + model_path = os.path.join(package_share_directory, 'models', 'screwdriver_obb_best.pt') + else: + model_path = os.path.join(package_share_directory, 'models', 'yolov8n-obb.pt') + + self.get_logger().info(f"Using OBB model type '{model_type}' from: {model_path}") + + # Load Model + self.model = YOLO(model_path) + if device_param: + try: + self.model.to(device_param) + self.get_logger().info(f"Model moved to device: {device_param}") + except Exception as e: + self.get_logger().warn(f"Failed to move model to device '{device_param}': {e}") + + # Optional override for class names + if class_names_param: + self.class_names = list(class_names_param) + self.get_logger().info(f"Using class names from parameter (n={len(self.class_names)})") + + # Determine image topic + if input_mode == 'robot': + image_topic = '/camera/color/image_raw' + elif input_mode == 'realsense': + image_topic = '/camera/camera/color/image_raw' + else: + self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") + image_topic = '/camera/camera/color/image_raw' + + # Create Lifecycle Publishers + self.annotated_image_pub = self.create_lifecycle_publisher(Image, '/annotated_image', 10) + self.detection_pub = self.create_lifecycle_publisher(Detection2DArray, '/detections', 10) + + # Create Subscription + self.image_sub = self.create_subscription(Image, image_topic, self.image_callback, 10) + + self.get_logger().info("Configuration complete.") + return TransitionCallbackReturn.SUCCESS + + except Exception as e: + self.get_logger().error(f"Failed to configure OBB Detector Node: {e}") + return TransitionCallbackReturn.FAILURE + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Activating OBB Detector Node...") + super().on_activate(state) + self._is_active = True + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Deactivating OBB Detector Node...") + self._is_active = False + super().on_deactivate(state) + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Cleaning up OBB Detector resources...") + + self.destroy_publisher(self.annotated_image_pub) + self.destroy_publisher(self.detection_pub) + self.destroy_subscription(self.image_sub) + + self.annotated_image_pub = self.detection_pub = self.image_sub = None + + self.model = None + if torch.cuda.is_available(): + torch.cuda.empty_cache() + + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Shutting down OBB Detector Node...") + if self.model is not None: + self.on_cleanup(state) + return TransitionCallbackReturn.SUCCESS + + def image_callback(self, msg): + if not self._is_active: + return + + try: + cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + except Exception as e: + self.get_logger().error(f'Image conversion error: {e}') + return + + # Run Inference + results = self.model(cv_image) + + detection_array_msg = Detection2DArray() + detection_array_msg.header = msg.header + + for result in results: + if not hasattr(result, 'obb') or result.obb is None: + continue + + # Filter by confidence using boolean masking + mask = result.obb.conf >= self.conf_threshold + result.obb = result.obb[mask] + + # Publish Annotated Image + annotated_image = result.plot() + try: + annotated_msg = self.bridge.cv2_to_imgmsg(annotated_image, encoding='bgr8') + annotated_msg.header = msg.header + self.annotated_image_pub.publish(annotated_msg) + except Exception as e: + self.get_logger().error(f'Annotated image conversion error: {e}') + + # Fill Detection2D Messages + for obb in result.obb: + detection_msg = Detection2D() + detection_msg.header = msg.header + + hypothesis = ObjectHypothesisWithPose() + cls_id = int(obb.cls.item()) + + if self.class_names and cls_id < len(self.class_names): + class_name = self.class_names[cls_id] + else: + class_name = self.model.names.get(cls_id, str(cls_id)) + + hypothesis.hypothesis.class_id = class_name + hypothesis.hypothesis.score = float(obb.conf.item()) + detection_msg.results.append(hypothesis) + + # OBB Bounding Box (XYWHR) + xywhr = obb.xywhr.cpu().numpy().flatten() + + detection_msg.bbox.center.position.x = float(xywhr[0]) + detection_msg.bbox.center.position.y = float(xywhr[1]) + detection_msg.bbox.center.theta = float(xywhr[4]) # Capture the rotation! + + detection_msg.bbox.size_x = float(xywhr[2]) + detection_msg.bbox.size_y = float(xywhr[3]) + + detection_array_msg.detections.append(detection_msg) + + self.detection_pub.publish(detection_array_msg) + +def main(args=None): + rclpy.init(args=args) + node = OBBDetectorLifecycleNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/r4s/src/perception/perception/action_obb_pose.py b/r4s/src/perception/perception/action_obb_pose.py new file mode 100644 index 0000000..d0282d0 --- /dev/null +++ b/r4s/src/perception/perception/action_obb_pose.py @@ -0,0 +1,234 @@ +import rclpy +from rclpy.lifecycle import Node as LifecycleNode, State, TransitionCallbackReturn +from sensor_msgs.msg import PointCloud2, PointField, Image +from vision_msgs.msg import Detection2DArray +from geometry_msgs.msg import PoseStamped, TransformStamped +from std_msgs.msg import Header + +import message_filters +from cv_bridge import CvBridge +import cv2 +import numpy as np +import struct +import sensor_msgs_py.point_cloud2 as pc2 +from transforms3d.quaternions import mat2quat +from tf2_ros import TransformBroadcaster + +from rclpy.qos import QoSProfile, ReliabilityPolicy + +class PointOBBCloudCropperLifecycleNode(LifecycleNode): + def __init__(self): + super().__init__('point_obb_cloud_cropper_node') + self.bridge = CvBridge() + + # Declare parameters + self.declare_parameter('input_mode', 'robot') + + # Placeholders + self.pc_pub = None + self.pose_pub = None + self.tf_broadcaster = None + + self.pc_sub = None + self.img_sub = None + self.det_sub = None + self.ts = None + + self._is_active = False + self.get_logger().info('PointCloud OBB Cropper Lifecycle Node Initialized (Unconfigured).') + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Configuring PointCloud OBB Cropper Node...") + + try: + input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + if input_mode == 'robot': + pc_topic = '/camera/depth/color/points' + img_topic = '/camera/color/image_raw' + else: + pc_topic = '/camera/camera/depth/color/points' + img_topic = '/camera/camera/color/image_raw' + + # Lifecycle Publishers + self.pc_pub = self.create_lifecycle_publisher(PointCloud2, '/cropped_pointcloud', 10) + self.pose_pub = self.create_lifecycle_publisher(PoseStamped, '/object_pose_screwdriver', 10) + self.tf_broadcaster = TransformBroadcaster(self) + + # Match the rosbag's reliable QoS + reliable_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE) + + # Message Filters Subscriptions + self.pc_sub = message_filters.Subscriber(self, PointCloud2, pc_topic, qos_profile=reliable_qos) + self.img_sub = message_filters.Subscriber(self, Image, img_topic, qos_profile=reliable_qos) + self.det_sub = message_filters.Subscriber(self, Detection2DArray, '/detections', qos_profile=reliable_qos) + + # Synchronizer + self.ts = message_filters.ApproximateTimeSynchronizer( + [self.pc_sub, self.det_sub, self.img_sub], + queue_size=500, + slop=1.5 + ) + self.ts.registerCallback(self.sync_callback) + + self.get_logger().info("Configuration complete.") + return TransitionCallbackReturn.SUCCESS + except Exception as e: + self.get_logger().error(f"Failed to configure OBB Cropper Node: {e}") + return TransitionCallbackReturn.FAILURE + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Activating PointCloud OBB Cropper Node...") + super().on_activate(state) + self._is_active = True + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Deactivating PointCloud OBB Cropper Node...") + self._is_active = False + super().on_deactivate(state) + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Cleaning up OBB Cropper resources...") + + self.destroy_publisher(self.pc_pub) + self.destroy_publisher(self.pose_pub) + + # Destroy underlying ROS subscriptions in message_filters + if self.pc_sub is not None: + self.destroy_subscription(self.pc_sub.sub) + if self.img_sub is not None: + self.destroy_subscription(self.img_sub.sub) + if self.det_sub is not None: + self.destroy_subscription(self.det_sub.sub) + + self.pc_pub = self.pose_pub = self.tf_broadcaster = None + self.pc_sub = self.img_sub = self.det_sub = self.ts = None + + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Shutting down OBB Cropper Node...") + if self.pc_pub is not None: + self.on_cleanup(state) + return TransitionCallbackReturn.SUCCESS + + def sync_callback(self, cloud_msg, detection_msg, image_msg): + if not self._is_active: + return + + # If there are no detections, do nothing + if not detection_msg.detections: + return + + try: + color_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8') + except Exception as e: + self.get_logger().error(f"Image conversion error: {e}") + return + + pc_width = cloud_msg.width + pc_height = cloud_msg.height + + cloud_points = np.array([ + [x, y, z] + for x, y, z in pc2.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=False) + ]).reshape((pc_height, pc_width, 3)) + + all_colored_points = [] + + for idx, detection in enumerate(detection_msg.detections): + detected_class = detection.results[0].hypothesis.class_id + cx = detection.bbox.center.position.x + cy = detection.bbox.center.position.y + theta = detection.bbox.center.theta + w = detection.bbox.size_x + h = detection.bbox.size_y + + angle_deg = np.degrees(theta) + rect = ((cx, cy), (w, h), angle_deg) + box = cv2.boxPoints(rect) + box = np.int32(box) + + mask = np.zeros((pc_height, pc_width), dtype=np.uint8) + cv2.fillPoly(mask, [box], 255) + + cropped_points = cloud_points[mask == 255] + cropped_colors = color_image[mask == 255] + + # Drop invalid depth points + valid_mask = ~np.isnan(cropped_points).any(axis=1) + cropped_points = cropped_points[valid_mask] + cropped_colors = cropped_colors[valid_mask] + + self.get_logger().info(f"Class '{detected_class}': Cropped {len(cropped_points)} valid 3D points.") + + if len(cropped_points) >= 3: + for pt, color in zip(cropped_points, cropped_colors): + x, y, z = pt + b, g, r = color + rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] + all_colored_points.append([x, y, z, rgb]) + + centroid = np.mean(cropped_points, axis=0) + centered = cropped_points - centroid + _, _, vh = np.linalg.svd(centered, full_matrices=False) + R = vh.T + + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = centroid + quat_wxyz = mat2quat(T[:3, :3]) + quat = [quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]] + + pose_msg = PoseStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = cloud_msg.header.frame_id + pose_msg.pose.position.x = float(centroid[0]) + pose_msg.pose.position.y = float(centroid[1]) + pose_msg.pose.position.z = float(centroid[2]) + pose_msg.pose.orientation.x = float(quat[0]) + pose_msg.pose.orientation.y = float(quat[1]) + pose_msg.pose.orientation.z = float(quat[2]) + pose_msg.pose.orientation.w = float(quat[3]) + self.pose_pub.publish(pose_msg) + + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = cloud_msg.header.frame_id + t.child_frame_id = f'object_frame_{idx}' + t.transform.translation.x = float(centroid[0]) + t.transform.translation.y = float(centroid[1]) + t.transform.translation.z = float(centroid[2]) + t.transform.rotation.x = float(quat[0]) + t.transform.rotation.y = float(quat[1]) + t.transform.rotation.z = float(quat[2]) + t.transform.rotation.w = float(quat[3]) + self.tf_broadcaster.sendTransform(t) + + if all_colored_points: + header = Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = cloud_msg.header.frame_id + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), + ] + cropped_pc = pc2.create_cloud(header, fields, all_colored_points) + self.pc_pub.publish(cropped_pc) + +def main(args=None): + rclpy.init(args=args) + node = PointOBBCloudCropperLifecycleNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/r4s/src/perception/perception/action_pose_pca.py b/r4s/src/perception/perception/action_pose_pca.py new file mode 100644 index 0000000..a8ed26f --- /dev/null +++ b/r4s/src/perception/perception/action_pose_pca.py @@ -0,0 +1,249 @@ +import rclpy +from rclpy.lifecycle import Node as LifecycleNode, State, TransitionCallbackReturn +from sensor_msgs.msg import PointCloud2, PointField, Image +from vision_msgs.msg import Detection2DArray +from geometry_msgs.msg import PoseStamped, TransformStamped +from std_msgs.msg import Header, Float32 +from visualization_msgs.msg import Marker + +import message_filters +from cv_bridge import CvBridge +import numpy as np +import struct +import sensor_msgs_py.point_cloud2 as pc2 +from transforms3d.quaternions import mat2quat +from tf2_ros import TransformBroadcaster + +from rclpy.qos import qos_profile_sensor_data, QoSProfile, ReliabilityPolicy + +class PointCloudCropperLifecycleNode(LifecycleNode): + def __init__(self): + super().__init__('pointcloud_cropper_node') + self.bridge = CvBridge() + + self.declare_parameter('input_mode', 'robot') + + self.pc_pub = None + self.pose_pub = None + self.marker_pub = None + self.radius_pub = None + self.tf_broadcaster = None + + self.pc_sub = None + self.img_sub = None + self.det_sub = None + self.ts = None + + self._is_active = False + self.get_logger().info('PointCloud Cropper Lifecycle Node Initialized (Unconfigured).') + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Configuring PointCloud Cropper Node...") + + try: + input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + if input_mode == 'robot': + pc_topic = '/camera/depth/color/points' + img_topic = '/camera/color/image_raw' + else: + pc_topic = '/camera/camera/depth/color/points' + img_topic = '/camera/camera/color/image_raw' + + # Lifecycle Publishers + self.pc_pub = self.create_lifecycle_publisher(PointCloud2, '/cropped_pointcloud', 10) + self.pose_pub = self.create_lifecycle_publisher(PoseStamped, '/object_pose', 10) + self.marker_pub = self.create_lifecycle_publisher(Marker, '/object_sphere_marker', 10) + self.radius_pub = self.create_lifecycle_publisher(Float32, '/perception/detected_object_radius', 10) + self.tf_broadcaster = TransformBroadcaster(self) + + # Subscriptions + self.pc_sub = message_filters.Subscriber(self, PointCloud2, pc_topic, qos_profile=qos_profile_sensor_data) + self.img_sub = message_filters.Subscriber(self, Image, img_topic, qos_profile=qos_profile_sensor_data) + reliable_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE) + self.det_sub = message_filters.Subscriber(self, Detection2DArray, '/detections', qos_profile=reliable_qos) + + # Synchronizer + self.ts = message_filters.ApproximateTimeSynchronizer( + [self.pc_sub, self.det_sub, self.img_sub], queue_size=100, slop=0.05 + ) + self.ts.registerCallback(self.sync_callback) + + self.get_logger().info("Configuration complete.") + return TransitionCallbackReturn.SUCCESS + except Exception as e: + self.get_logger().error(f"Failed to configure Cropper Node: {e}") + return TransitionCallbackReturn.FAILURE + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Activating PointCloud Cropper Node...") + super().on_activate(state) + self._is_active = True + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Deactivating PointCloud Cropper Node...") + self._is_active = False + super().on_deactivate(state) + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Cleaning up Cropper resources...") + + self.destroy_publisher(self.pc_pub) + self.destroy_publisher(self.pose_pub) + self.destroy_publisher(self.marker_pub) + self.destroy_publisher(self.radius_pub) + + if self.pc_sub is not None: self.destroy_subscription(self.pc_sub.sub) + if self.img_sub is not None: self.destroy_subscription(self.img_sub.sub) + if self.det_sub is not None: self.destroy_subscription(self.det_sub.sub) + + self.pc_pub = self.pose_pub = self.marker_pub = self.radius_pub = self.tf_broadcaster = None + self.pc_sub = self.img_sub = self.det_sub = self.ts = None + + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + if self.pc_pub is not None: + self.on_cleanup(state) + return TransitionCallbackReturn.SUCCESS + + def sync_callback(self, cloud_msg, detection_msg, image_msg): + if not self._is_active: return + + try: + color_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8') + except Exception as e: + self.get_logger().error(f"Image conversion error: {e}") + return + + pc_width, pc_height = cloud_msg.width, cloud_msg.height + cloud_points = np.array([ + [x, y, z] for x, y, z in pc2.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=False) + ]).reshape((pc_height, pc_width, 3)) + + all_colored_points = [] + + for idx, detection in enumerate(detection_msg.detections): + detected_class = detection.results[0].hypothesis.class_id + cx, cy = int(detection.bbox.center.position.x), int(detection.bbox.center.position.y) + w, h = int(detection.bbox.size_x), int(detection.bbox.size_y) + + xmin, xmax = max(cx - w // 2, 0), min(cx + w // 2, pc_width) + ymin, ymax = max(cy - h // 2, 0), min(cy + h // 2, pc_height) + + cropped_points = cloud_points[ymin:ymax, xmin:xmax, :].reshape(-1, 3) + cropped_colors = color_image[ymin:ymax, xmin:xmax, :].reshape(-1, 3) + + valid_mask = ~np.isnan(cropped_points).any(axis=1) + cropped_points = cropped_points[valid_mask] + cropped_colors = cropped_colors[valid_mask] + + for pt, color in zip(cropped_points, cropped_colors): + x, y, z = pt + b, g, r = color + rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] + all_colored_points.append([x, y, z, rgb]) + + self.get_logger().info( + f"Cropped '{detected_class}' object {idx}: [{xmin}:{xmax}, {ymin}:{ymax}] -> {len(cropped_points)} valid points" + ) + + if len(cropped_points) >= 3: + centroid = np.mean(cropped_points, axis=0) + centered = cropped_points - centroid + _, _, vh = np.linalg.svd(centered, full_matrices=False) + R = vh.T + + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = centroid + quat_wxyz = mat2quat(T[:3, :3]) + quat = [quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]] + + # Pose Publish + pose_msg = PoseStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = cloud_msg.header.frame_id + pose_msg.pose.position.x = float(centroid[0]) + pose_msg.pose.position.y = float(centroid[1]) + pose_msg.pose.position.z = float(centroid[2]) + pose_msg.pose.orientation.x = float(quat[0]) + pose_msg.pose.orientation.y = float(quat[1]) + pose_msg.pose.orientation.z = float(quat[2]) + pose_msg.pose.orientation.w = float(quat[3]) + self.pose_pub.publish(pose_msg) + + # TF Publish + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = cloud_msg.header.frame_id + t.child_frame_id = f'object_frame_{idx}' + t.transform.translation.x = float(centroid[0]) + t.transform.translation.y = float(centroid[1]) + t.transform.translation.z = float(centroid[2]) + t.transform.rotation.x = float(quat[0]) + t.transform.rotation.y = float(quat[1]) + t.transform.rotation.z = float(quat[2]) + t.transform.rotation.w = float(quat[3]) + self.tf_broadcaster.sendTransform(t) + + # Radius & Sphere Logic + distances = np.linalg.norm(cropped_points - centroid, axis=1) + metric_radius = float(np.percentile(distances, 98)) + + radius_msg = Float32() + radius_msg.data = metric_radius + self.radius_pub.publish(radius_msg) + + sphere_diameter = metric_radius * 2.0 + marker = Marker() + marker.header = cloud_msg.header + marker.ns = "object_spheres" + marker.id = idx + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.pose.position.x = float(centroid[0]) + marker.pose.position.y = float(centroid[1]) + marker.pose.position.z = float(centroid[2]) + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + marker.scale.x = sphere_diameter + marker.scale.y = sphere_diameter + marker.scale.z = sphere_diameter + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 0.5 + self.marker_pub.publish(marker) + + if all_colored_points: + header = Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = cloud_msg.header.frame_id + + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), + ] + + cropped_pc = pc2.create_cloud(header, fields, all_colored_points) + self.pc_pub.publish(cropped_pc) + +def main(args=None): + rclpy.init(args=args) + node = PointCloudCropperLifecycleNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/r4s/src/perception/perception/action_vision_manager.py b/r4s/src/perception/perception/action_vision_manager.py new file mode 100644 index 0000000..e8d73b1 --- /dev/null +++ b/r4s/src/perception/perception/action_vision_manager.py @@ -0,0 +1,450 @@ +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.signals import SignalHandlerOptions +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Float32 + +from lifecycle_msgs.srv import ChangeState +from lifecycle_msgs.msg import Transition + +from my_robot_interfaces.action import RunVision + +import asyncio +import threading + +class PerceptionDispatcher(Node): + def __init__(self): + super().__init__('perception_pipeline_node') + self.group = ReentrantCallbackGroup() + + # Action Server setup + self._action_server = ActionServer( + self, + RunVision, + 'run_perception_pipeline', + execute_callback=self.execute_callback, + goal_callback=self.goal_callback, + cancel_callback=self.cancel_callback, + callback_group=self.group + ) + + # ========================================== + # 1. LIFECYCLE CLIENTS (Isolated Pipelines) + # ========================================== + self.table_lifecycle_client = self.create_client(ChangeState, '/table_height_estimator/change_state', callback_group=self.group) + + # Pipeline A: Screws + self.yolo_screw_client = self.create_client(ChangeState, '/yolo_screw_node/change_state', callback_group=self.group) + self.cropper_screw_client = self.create_client(ChangeState, '/cropper_screw_node/change_state', callback_group=self.group) + + # Pipeline B: Car Objects + self.yolo_car_client = self.create_client(ChangeState, '/yolo_car_node/change_state', callback_group=self.group) + self.cropper_car_client = self.create_client(ChangeState, '/cropper_car_node/change_state', callback_group=self.group) + + # OBB Pipeline: Screwdriver + self.obb_object_lifecycle_client = self.create_client(ChangeState, '/obb_detector_node/change_state', callback_group=self.group) + self.obb_cropper_lifecycle_client = self.create_client(ChangeState, '/point_obb_cloud_cropper_node/change_state', callback_group=self.group) + + # Subdoor Pose Estimation + self.subdoor_lifecycle_client = self.create_client(ChangeState, '/subdoor_pose_estimator/change_state', callback_group=self.group) + + # Place Object (Table Segmentation) + self.place_object_client = self.create_client(ChangeState, '/place_object_node/change_state', callback_group=self.group) + # ========================================== + # 2. PERSISTENT STATE STORAGE + # ========================================== + # This dictionary stores the latest data for every task safely + self.vision_data = { + 'screw': {'pose': None, 'radius': None}, + 'car object': {'pose': None, 'radius': None}, + 'table': None, + 'subdoor': None, + 'screwdriver': None, + 'place_pose': None + } + + # ========================================== + # 3. PERSISTENT SUBSCRIBERS + # ========================================== + self.create_subscription(PoseStamped, '/screw/pose', self.screw_pose_cb, 10, callback_group=self.group) + self.create_subscription(Float32, '/screw/radius', self.screw_rad_cb, 10, callback_group=self.group) + self.create_subscription(PoseStamped, '/car/pose', self.car_pose_cb, 10, callback_group=self.group) + self.create_subscription(Float32, '/car/radius', self.car_rad_cb, 10, callback_group=self.group) + self.create_subscription(Float32, '/table_height_value', self.table_cb, 10, callback_group=self.group) + self.create_subscription(MarkerArray, '/body_markers', self.subdoor_cb, 10, callback_group=self.group) + self.create_subscription(PoseStamped, '/object_pose_screwdriver', self.screwdriver_cb, 10, callback_group=self.group) + + # NEW: Listen to the placement pose calculated by the table segmentation node + self.create_subscription(PoseStamped, '/perception/target_place_pose', self.place_pose_cb, 10, callback_group=self.group) + + self.get_logger().info("Perception Dispatcher initializing...") + threading.Thread(target=self._run_startup_routine, daemon=True).start() + + # --- SUBSCRIBER CALLBACKS --- + def screw_pose_cb(self, msg): self.vision_data['screw']['pose'] = msg + def screw_rad_cb(self, msg): self.vision_data['screw']['radius'] = msg.data + def car_pose_cb(self, msg): self.vision_data['car object']['pose'] = msg + def car_rad_cb(self, msg): self.vision_data['car object']['radius'] = msg.data + def screwdriver_cb(self, msg): self.vision_data['screwdriver'] = msg + def place_pose_cb(self, msg): self.vision_data['place_pose'] = msg + + def table_cb(self, msg: Float32): + self.vision_data['table'] = msg.data + + def subdoor_cb(self, msg: MarkerArray): + if msg.markers and msg.markers[0].ns == "corners": + self.vision_data['subdoor'] = msg.markers + + # --- STARTUP ROUTINE --- + def _run_startup_routine(self): + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + try: + # Use a variable to track overall success + all_successful = loop.run_until_complete(self._startup_all()) + + if all_successful: + self.get_logger().info("======================================================") + self.get_logger().info("Startup routine completed. ALL nodes pre-configured.") + self.get_logger().info("Perception Dispatcher is READY for tasks.") + self.get_logger().info("======================================================") + else: + self.get_logger().error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") + self.get_logger().error("STARTUP PARTIALLY FAILED. Some nodes did not configure.") + self.get_logger().error("Check the logs above for specific node failures.") + self.get_logger().error("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") + finally: + loop.close() + + async def _startup_all(self): + results = await asyncio.gather( + self._configure_and_log(self.table_lifecycle_client, 'Table Height Node'), + self._configure_and_log(self.yolo_screw_client, 'YOLO Screw Node'), + self._configure_and_log(self.cropper_screw_client, 'Cropper Screw Node'), + self._configure_and_log(self.yolo_car_client, 'YOLO Car Node'), + self._configure_and_log(self.cropper_car_client, 'Cropper Car Node'), + self._configure_and_log(self.subdoor_lifecycle_client, 'Subdoor Pose Node'), + self._configure_and_log(self.obb_object_lifecycle_client, 'OBB Detector Node'), + self._configure_and_log(self.obb_cropper_lifecycle_client, 'OBB Cropper Node'), + self._configure_and_log(self.place_object_client, 'Place Object Node'), + ) + return all(results) + + async def _configure_and_log(self, client, node_name: str): + self.get_logger().info(f"Pre-configuring {node_name}...") + success = await self._change_lifecycle_state(client, Transition.TRANSITION_CONFIGURE) + if success: + self.get_logger().info(f"SUCCESS: {node_name} Configured.") + else: + self.get_logger().error(f"FAILURE: Could not configure {node_name}.") + return success + + async def _change_lifecycle_state(self, client, transition_id): + if not client.wait_for_service(timeout_sec=5.0): + self.get_logger().error(f"Service {client.srv_name} not available.") + return False + + req = ChangeState.Request() + req.transition.id = transition_id + future = client.call_async(req) + + while not future.done(): + await asyncio.sleep(0.05) + + result = future.result() + return result.success if result is not None else False + + # --- ACTION SERVER CALLBACKS --- + def goal_callback(self, goal_request): + self.get_logger().info(f"Received request for task: {goal_request.task_name}") + return GoalResponse.ACCEPT + + def cancel_callback(self, goal_handle): + self.get_logger().warn("Task canceled by client.") + return CancelResponse.ACCEPT + + def execute_callback(self, goal_handle): + task = goal_handle.request.task_name + self.get_logger().info(f"Executing task: {task}...") + + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + + result = RunVision.Result() + result.success = False + + try: + if task == 'table_height': + result = loop.run_until_complete(self.handle_table_height(goal_handle)) + elif task == 'detect_screws': + # Pass the screw clients to the generic YOLO handler + result = loop.run_until_complete(self.handle_detect_yolo(goal_handle, self.yolo_screw_client, self.cropper_screw_client, "screw")) + elif task == 'car_objects': + # Pass the car clients to the generic YOLO handler + result = loop.run_until_complete(self.handle_detect_yolo(goal_handle, self.yolo_car_client, self.cropper_car_client, "car object")) + elif task == 'subdoor_pose': + result = loop.run_until_complete(self.handle_subdoor_pose(goal_handle)) + elif task == 'detect_screwdriver': + result = loop.run_until_complete(self.handle_detect_screwdriver(goal_handle)) + elif task == 'place_object': + result = loop.run_until_complete(self.handle_place_object(goal_handle)) + else: + result.message = f"Unknown task: {task}" + except Exception as e: + self.get_logger().error(f"Task failed with error: {e}") + result.success = False + result.message = str(e) if str(e) else type(e).__name__ + finally: + loop.close() + + if result.success: + goal_handle.succeed() + else: + try: + goal_handle.abort() + except Exception: + pass + + return result + + # --- TASK HANDLERS --- + async def handle_table_height(self, goal_handle): + feedback_msg, result_msg = RunVision.Feedback(), RunVision.Result() + self.vision_data['table'] = None # Clear old data + + feedback_msg.current_phase = "Activating Table Height Node..." + goal_handle.publish_feedback(feedback_msg) + if not await self._change_lifecycle_state(self.table_lifecycle_client, Transition.TRANSITION_ACTIVATE): + result_msg.success, result_msg.message = False, "Failed to activate table node." + return result_msg + + feedback_msg.current_phase = "Waiting for vision data..." + goal_handle.publish_feedback(feedback_msg) + + for _ in range(150): + if self.vision_data['table'] is not None: break + await asyncio.sleep(0.1) + + feedback_msg.current_phase = "Deactivating Table Height Node..." + goal_handle.publish_feedback(feedback_msg) + await self._change_lifecycle_state(self.table_lifecycle_client, Transition.TRANSITION_DEACTIVATE) + + if self.vision_data['table'] is not None: + result_msg.success, result_msg.message = True, f"Table Height: {self.vision_data['table']:.3f} meters" + else: + result_msg.success, result_msg.message = False, "Vision processing timed out." + return result_msg + + async def handle_detect_yolo(self, goal_handle, yolo_client, cropper_client, object_name): + feedback_msg, result_msg = RunVision.Feedback(), RunVision.Result() + + # Clear old data for this specific pipeline + self.vision_data[object_name]['pose'] = None + self.vision_data[object_name]['radius'] = None + + feedback_msg.current_phase = f"Activating Pipeline ({object_name})..." + goal_handle.publish_feedback(feedback_msg) + + yolo_success = await self._change_lifecycle_state(yolo_client, Transition.TRANSITION_ACTIVATE) + cropper_success = await self._change_lifecycle_state(cropper_client, Transition.TRANSITION_ACTIVATE) + + if not (yolo_success and cropper_success): + result_msg.success, result_msg.message = False, "Failed to activate YOLO/Cropper." + return result_msg + + feedback_msg.current_phase = f"Waiting for {object_name} detection..." + goal_handle.publish_feedback(feedback_msg) + + for _ in range(150): + if self.vision_data[object_name]['pose'] is not None and self.vision_data[object_name]['radius'] is not None: + break + await asyncio.sleep(0.1) + + feedback_msg.current_phase = f"Deactivating Pipeline ({object_name})..." + goal_handle.publish_feedback(feedback_msg) + + await self._change_lifecycle_state(cropper_client, Transition.TRANSITION_DEACTIVATE) + await self._change_lifecycle_state(yolo_client, Transition.TRANSITION_DEACTIVATE) + + pose = self.vision_data[object_name]['pose'] + radius = self.vision_data[object_name]['radius'] + + if pose is not None and radius is not None: + result_msg.success = True + x, y, z = pose.pose.position.x, pose.pose.position.y, pose.pose.position.z + result_msg.message = f"{object_name.capitalize()} detected at [x: {x:.3f}, y: {y:.3f}, z: {z:.3f}] with Radius: {radius:.4f}m" + else: + result_msg.success, result_msg.message = False, f"Timed out waiting for {object_name}." + + return result_msg + + async def handle_subdoor_pose(self, goal_handle): + feedback_msg, result_msg = RunVision.Feedback(), RunVision.Result() + self.vision_data['subdoor'] = None + + feedback_msg.current_phase = "Activating Subdoor Node..." + goal_handle.publish_feedback(feedback_msg) + if not await self._change_lifecycle_state(self.subdoor_lifecycle_client, Transition.TRANSITION_ACTIVATE): + result_msg.success, result_msg.message = False, "Failed to activate subdoor." + return result_msg + + feedback_msg.current_phase = "Waiting for subdoor..." + goal_handle.publish_feedback(feedback_msg) + + for _ in range(150): + if self.vision_data['subdoor'] is not None: break + await asyncio.sleep(0.1) + + feedback_msg.current_phase = "Deactivating Subdoor Node..." + goal_handle.publish_feedback(feedback_msg) + await self._change_lifecycle_state(self.subdoor_lifecycle_client, Transition.TRANSITION_DEACTIVATE) + + markers = self.vision_data['subdoor'] + if markers is not None: + result_msg.success = True + pose_info = [f"Subdoor estimated successfully with {len(markers)} corner markers."] + for i, marker in enumerate(markers): + x, y, z = marker.pose.position.x, marker.pose.position.y, marker.pose.position.z + pose_info.append(f"Pose {i}: x={x:.3f}, y={y:.3f}, z={z:.3f}") + result_msg.message = "\n".join(pose_info) + else: + result_msg.success, result_msg.message = False, "Timed out waiting for subdoor." + + return result_msg + + async def handle_detect_screwdriver(self, goal_handle): + feedback_msg, result_msg = RunVision.Feedback(), RunVision.Result() + self.vision_data['screwdriver'] = None + + feedback_msg.current_phase = "Activating OBB Pipeline..." + goal_handle.publish_feedback(feedback_msg) + + obb_success = await self._change_lifecycle_state(self.obb_object_lifecycle_client, Transition.TRANSITION_ACTIVATE) + cropper_success = await self._change_lifecycle_state(self.obb_cropper_lifecycle_client, Transition.TRANSITION_ACTIVATE) + + if not (obb_success and cropper_success): + result_msg.success, result_msg.message = False, "Failed to activate OBB nodes." + return result_msg + + feedback_msg.current_phase = "Waiting for screwdriver..." + goal_handle.publish_feedback(feedback_msg) + + for _ in range(150): + if self.vision_data['screwdriver'] is not None: break + await asyncio.sleep(0.1) + + feedback_msg.current_phase = "Deactivating OBB Pipeline..." + goal_handle.publish_feedback(feedback_msg) + + await self._change_lifecycle_state(self.obb_cropper_lifecycle_client, Transition.TRANSITION_DEACTIVATE) + await self._change_lifecycle_state(self.obb_object_lifecycle_client, Transition.TRANSITION_DEACTIVATE) + + pose = self.vision_data['screwdriver'] + if pose is not None: + result_msg.success = True + x, y, z = pose.pose.position.x, pose.pose.position.y, pose.pose.position.z + qx, qy, qz, qw = pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w + result_msg.message = (f"Screwdriver detected at [x: {x:.3f}, y: {y:.3f}, z: {z:.3f}] " + f"Orientation: [qx: {qx:.3f}, qy: {qy:.3f}, qz: {qz:.3f}, qw: {qw:.3f}]") + else: + result_msg.success, result_msg.message = False, "Timed out waiting for screwdriver." + + return result_msg + + async def handle_place_object(self, goal_handle): + feedback_msg, result_msg = RunVision.Feedback(), RunVision.Result() + self.vision_data['place_pose'] = None + + feedback_msg.current_phase = "Activating Table Segmentation (Place) Node..." + goal_handle.publish_feedback(feedback_msg) + + if not await self._change_lifecycle_state(self.place_object_client, Transition.TRANSITION_ACTIVATE): + result_msg.success, result_msg.message = False, "Failed to activate Place Object node." + return result_msg + + feedback_msg.current_phase = "Scanning table for safe dropping zone..." + goal_handle.publish_feedback(feedback_msg) + + # Wait up to 15 seconds for an empty spot to be found + for _ in range(150): + if self.vision_data['place_pose'] is not None: break + await asyncio.sleep(0.1) + + feedback_msg.current_phase = "Deactivating Place Object Node..." + goal_handle.publish_feedback(feedback_msg) + await self._change_lifecycle_state(self.place_object_client, Transition.TRANSITION_DEACTIVATE) + + pose = self.vision_data['place_pose'] + if pose is not None: + result_msg.success = True + x, y, z = pose.pose.position.x, pose.pose.position.y, pose.pose.position.z + qx, qy, qz, qw = pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w + result_msg.message = (f"Safe Placement Pose found at [x: {x:.3f}, y: {y:.3f}, z: {z:.3f}]\n" + f"Orientation: [qx: {qx:.3f}, qy: {qy:.3f}, qz: {qz:.3f}, qw: {qw:.3f}]") + else: + result_msg.success, result_msg.message = False, "Timed out waiting for an empty spot on the table." + + return result_msg + + # --- SHUTDOWN ROUTINES --- + def sync_shutdown_routine(self): + self.get_logger().info('Initiating graceful shutdown of all pipeline nodes...') + managed_nodes = [ + ('YOLO Screw Node', self.yolo_screw_client), + ('YOLO Car Node', self.yolo_car_client), + ('Cropper Screw Node', self.cropper_screw_client), + ('Cropper Car Node', self.cropper_car_client), + ('Table Height Node', self.table_lifecycle_client), + ('Subdoor Pose Node', self.subdoor_lifecycle_client), + ('OBB Detector Node', self.obb_object_lifecycle_client), + ('OBB Cropper Node', self.obb_cropper_lifecycle_client), + ('Place Object Node', self.place_object_client), + + ] + + for name, client in managed_nodes: + self.get_logger().info(f' Cleaning up {name}...') + self._sync_change_state(client, Transition.TRANSITION_CLEANUP) + self.get_logger().info(f' Shutting down {name}...') + self._sync_change_state(client, Transition.TRANSITION_UNCONFIGURED_SHUTDOWN) + + self.get_logger().info('All managed nodes shut down cleanly.') + + def _sync_change_state(self, client, transition_id): + if not client.wait_for_service(timeout_sec=1.0): + self.get_logger().warn(f"Service {client.srv_name} not available during shutdown.") + return False + + req = ChangeState.Request() + req.transition.id = transition_id + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future, timeout_sec=2.0) + + if future.result() is not None: + return future.result().success + return False + +def main(args=None): + rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) + + node = PerceptionDispatcher() + executor = MultiThreadedExecutor() + executor.add_node(node) + + try: + while rclpy.ok(): + executor.spin_once(timeout_sec=0.1) + except KeyboardInterrupt: + node.get_logger().info("Ctrl+C caught! Beginning graceful shutdown...") + finally: + node.sync_shutdown_routine() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/r4s/src/perception/perception/action_yolo_object_detection.py b/r4s/src/perception/perception/action_yolo_object_detection.py new file mode 100644 index 0000000..bfa7a47 --- /dev/null +++ b/r4s/src/perception/perception/action_yolo_object_detection.py @@ -0,0 +1,221 @@ +import rclpy +from rclpy.lifecycle import Node as LifecycleNode, State, TransitionCallbackReturn +from sensor_msgs.msg import Image +from std_msgs.msg import Float32 +from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose +from cv_bridge import CvBridge +from ultralytics import YOLO +from ultralytics.engine.results import Boxes +import torch +import cv2 +import os +from ament_index_python.packages import get_package_share_directory +from pathlib import Path + +from rclpy.signals import SignalHandlerOptions + +def get_package_name_from_path(file_path): + p = Path(file_path) + package_parts = p.parts[p.parts.index('site-packages') + 1:] + return package_parts[0] + +class YoloDetectorLifecycleNode(LifecycleNode): + def __init__(self): + self.package_name = get_package_name_from_path(__file__) + super().__init__('yolo_detector_node') + self.bridge = CvBridge() + + # Declare parameters + self.declare_parameter('model_type', 'fine_tuned') + self.declare_parameter('input_mode', 'robot') + self.declare_parameter('model_path', '') + self.declare_parameter('conf_threshold', 0.6) + self.declare_parameter('device', '') + self.declare_parameter('class_names', []) + + # Placeholders for resources + self.model = None + self.class_names = None + self.image_sub = None + self.annotated_image_pub = None + self.detection_pub = None + self.max_dim_pub = None + self.conf_threshold = 0.6 + + self._is_active = False + self.get_logger().info('YOLO Detector Lifecycle Node Initialized (Unconfigured).') + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Configuring YOLO Node...") + + try: + model_type = self.get_parameter('model_type').get_parameter_value().string_value + input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + explicit_model_path = self.get_parameter('model_path').get_parameter_value().string_value + self.conf_threshold = self.get_parameter('conf_threshold').get_parameter_value().double_value + device_param = self.get_parameter('device').get_parameter_value().string_value + class_names_param = self.get_parameter('class_names').get_parameter_value().string_array_value + + if explicit_model_path: + model_path = explicit_model_path + else: + package_share_directory = get_package_share_directory(self.package_name) + model_map = { + 'fine_tuned': 'fine_tuned.pt', + 'screw': 'screw_best.pt', + 'default': 'yolov8n.pt' + } + model_filename = model_map.get(model_type, 'yolov8n.pt') + model_path = os.path.join(package_share_directory, 'models', model_filename) + + self.get_logger().info(f"Using model type '{model_type}' from: {model_path}") + + self.model = YOLO(model_path) + if device_param: + try: + self.model.to(device_param) + self.get_logger().info(f"Model moved to device: {device_param}") + except Exception as e: + self.get_logger().warn(f"Failed to move model to device '{device_param}': {e}") + + if class_names_param: + self.class_names = list(class_names_param) + + if input_mode == 'robot': + image_topic = '/camera/color/image_raw' + else: + image_topic = '/camera/camera/color/image_raw' + + # Lifecycle Publishers + self.annotated_image_pub = self.create_lifecycle_publisher(Image, '/annotated_image', 10) + self.detection_pub = self.create_lifecycle_publisher(Detection2DArray, '/detections', 10) + self.max_dim_pub = self.create_lifecycle_publisher(Float32, '/detections/max_dimension', 10) + + # Subscriptions + self.image_sub = self.create_subscription(Image, image_topic, self.image_callback, 10) + + self.get_logger().info("Configuration complete.") + return TransitionCallbackReturn.SUCCESS + + except Exception as e: + self.get_logger().error(f"Failed to configure YOLO Node: {e}") + return TransitionCallbackReturn.FAILURE + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Activating YOLO Node...") + super().on_activate(state) + self._is_active = True + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Deactivating YOLO Node...") + self._is_active = False + super().on_deactivate(state) + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Cleaning up YOLO resources...") + + self.destroy_publisher(self.annotated_image_pub) + self.destroy_publisher(self.detection_pub) + self.destroy_publisher(self.max_dim_pub) + self.destroy_subscription(self.image_sub) + + self.annotated_image_pub = self.detection_pub = self.max_dim_pub = self.image_sub = None + self.model = None + if torch.cuda.is_available(): + torch.cuda.empty_cache() + + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Shutting down YOLO Node...") + if self.model is not None: + self.on_cleanup(state) + return TransitionCallbackReturn.SUCCESS + + def image_callback(self, msg): + if not self._is_active: + return + + try: + cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + except Exception as e: + self.get_logger().error(f'Image conversion error: {e}') + return + + results = self.model(cv_image, verbose=False) + + detection_array_msg = Detection2DArray() + detection_array_msg.header = msg.header + + current_frame_max_dim = 0.0 + + for result in results: + filtered_boxes = [box for box in result.boxes if float(box.conf) >= self.conf_threshold] + + if filtered_boxes: + box_data = torch.stack([b.data[0] for b in filtered_boxes]) + result.boxes = Boxes(box_data, orig_shape=result.orig_shape) + else: + result.boxes = Boxes(torch.empty((0, 6)), orig_shape=result.orig_shape) + + annotated_image = result.plot() + try: + annotated_msg = self.bridge.cv2_to_imgmsg(annotated_image, encoding='bgr8') + annotated_msg.header = msg.header + self.annotated_image_pub.publish(annotated_msg) + except Exception as e: + self.get_logger().error(f'Annotated image conversion error: {e}') + + for box in filtered_boxes: + detection_msg = Detection2D() + detection_msg.header = msg.header + + hypothesis = ObjectHypothesisWithPose() + cls_id = int(box.cls) + + if self.class_names and cls_id < len(self.class_names): + class_name = self.class_names[cls_id] + else: + class_name = self.model.names.get(cls_id, str(cls_id)) + + hypothesis.hypothesis.class_id = class_name + hypothesis.hypothesis.score = float(box.conf) + detection_msg.results.append(hypothesis) + + xywh = box.xywh.cpu().numpy().flatten() + w = float(xywh[2]) + h = float(xywh[3]) + detection_msg.bbox.center.position.x = float(xywh[0]) + detection_msg.bbox.center.position.y = float(xywh[1]) + detection_msg.bbox.size_x = w + detection_msg.bbox.size_y = h + + # Track the largest dimension + max_dim = max(w, h) + if max_dim > current_frame_max_dim: + current_frame_max_dim = max_dim + + detection_array_msg.detections.append(detection_msg) + + self.detection_pub.publish(detection_array_msg) + + if detection_array_msg.detections: + dim_msg = Float32() + dim_msg.data = current_frame_max_dim + self.max_dim_pub.publish(dim_msg) + +def main(args=None): + rclpy.init(args=args) + node = YoloDetectorLifecycleNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/r4s/src/perception/perception/asjad_table_height_node.py b/r4s/src/perception/perception/asjad_table_height_node.py new file mode 100644 index 0000000..0aaa2f1 --- /dev/null +++ b/r4s/src/perception/perception/asjad_table_height_node.py @@ -0,0 +1,356 @@ +import rclpy +import rclpy.duration +from rclpy.lifecycle import Node as LifecycleNode, State, TransitionCallbackReturn +from sensor_msgs.msg import PointCloud2 +from visualization_msgs.msg import Marker, MarkerArray +import sensor_msgs_py.point_cloud2 as pc2 +import numpy as np +import open3d as o3d +from sklearn.linear_model import RANSACRegressor +from std_msgs.msg import Float32 +import tf2_ros +from scipy.spatial.transform import Rotation + +# Robot world frame — Z points up, gravity-aligned +WORLD_FRAME = 'eddie_base_footprint' + + +class TableHeightNode(LifecycleNode): + def __init__(self): + super().__init__('table_height_estimator') + + self.declare_parameter('input_mode', 'realsense') + self.declare_parameter('max_distance_m', 2.5) + input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + self.max_distance = float(self.get_parameter('max_distance_m').get_parameter_value().double_value) + + if input_mode == 'robot': + self.pc_topic = '/camera/depth/color/points' + elif input_mode == 'realsense': + self.pc_topic = '/camera/camera/depth/color/points' + else: + self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") + self.pc_topic = '/camera/camera/depth/color/points' + + self.det_model = None + self.marker_pub = None + self.debug_pc_pub = None + self.height_pub = None + self.pc_sub = None + self.tf_buffer = None + self.tf_listener = None + self._is_active = False + self.get_logger().info("Table Height Lifecycle Node Initialized (Unconfigured).") + + # --- LIFECYCLE CALLBACKS --- + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Configuring: lightweight geometric mode (no heavy models)") + try: + self.marker_pub = self.create_lifecycle_publisher(MarkerArray, '/table_height_visualization', 10) + self.debug_pc_pub = self.create_lifecycle_publisher(PointCloud2, '/table_points_debug', 10) + self.height_pub = self.create_lifecycle_publisher(Float32, '/table_height_value', 10) + self.pc_sub = self.create_subscription(PointCloud2, self.pc_topic, self.callback, 10) + + # TF buffer + listener for world-frame transform + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + self.get_logger().info("Configuration complete.") + self.get_logger().info("returned 1") + return TransitionCallbackReturn.SUCCESS + except Exception as e: + self.get_logger().error(f"Failed to configure: {e}") + return TransitionCallbackReturn.FAILURE + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Activating node...") + super().on_activate(state) + self._is_active = True + self.get_logger().info("Node activated. Processing incoming data.") + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Deactivating node...") + self._is_active = False + super().on_deactivate(state) + self.get_logger().info("Node deactivated. Pausing processing.") + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Cleaning up resources...") + self.destroy_publisher(self.marker_pub) + self.destroy_publisher(self.debug_pc_pub) + self.destroy_publisher(self.height_pub) + if self.pc_sub is not None: + self.destroy_subscription(self.pc_sub) + self.marker_pub = self.debug_pc_pub = self.height_pub = None + self.pc_sub = None + self.tf_buffer = None + self.tf_listener = None + self.get_logger().info("Cleanup complete.") + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Shutting down node...") + if self.det_model is not None: + self.on_cleanup(state) + return TransitionCallbackReturn.SUCCESS + + # --- HELPERS --- + + def _apply_transform(self, points, transform_stamped): + """Apply a TF TransformStamped to an Nx3 numpy array of points.""" + t = transform_stamped.transform.translation + q = transform_stamped.transform.rotation + translation = np.array([t.x, t.y, t.z]) + rot = Rotation.from_quat([q.x, q.y, q.z, q.w]) + return rot.apply(points) + translation + + # --- PROCESSING CALLBACK --- + + def callback(self, pc_msg): + if not self._is_active: + return + self.process_point_cloud(pc_msg) + + def process_point_cloud(self, pc_msg): + if pc_msg.height * pc_msg.width == 0: + return + + xyz_data = pc2.read_points_numpy(pc_msg, field_names=("x", "y", "z"), skip_nans=True) + if xyz_data.size == 0 or len(xyz_data.shape) != 2 or xyz_data.shape[1] < 3: + return + + valid_distances = np.linalg.norm(xyz_data, axis=1) < self.max_distance + points = xyz_data[valid_distances] + if len(points) < 500: + return + + # --- Try to transform points into the world frame via TF --- + use_world_frame = False + work_points = points + marker_frame = pc_msg.header.frame_id + + if self.tf_buffer is not None: + try: + transform = self.tf_buffer.lookup_transform( + WORLD_FRAME, + pc_msg.header.frame_id, + pc_msg.header.stamp, + timeout=rclpy.duration.Duration(seconds=0.1) + ) + work_points = self._apply_transform(points, transform) + use_world_frame = True + marker_frame = WORLD_FRAME + except Exception: + pass # fall back to camera-frame processing + + if use_world_frame: + self.get_logger().debug("Using world frame (TF available)") + else: + self.get_logger().debug("Using camera frame (no TF)") + + # up_idx: axis index for "vertical" direction + # world frame: Z (index 2) is up + # camera frame: Y (index 1) is up (approximate) + up_idx = 2 if use_world_frame else 1 + up_vector = np.array([0, 0, 1]) if use_world_frame else np.array([0, 1, 0]) + + # --- In world frame: restrict RANSAC to table-height points only --- + # This prevents RANSAC picking walls or robot body as dominant plane + if use_world_frame: + floor_approx = np.percentile(work_points[:, 2], 5) + table_mask = (work_points[:, 2] > floor_approx + 0.2) & \ + (work_points[:, 2] < floor_approx + 1.2) + ransac_pts = work_points[table_mask] + if len(ransac_pts) < 300: + ransac_pts = work_points # fall back to all points + else: + ransac_pts = work_points + + # --- Downsample and find dominant plane (table surface) --- + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(ransac_pts.astype(np.float64)) + pcd_down = pcd.voxel_down_sample(voxel_size=0.015) + + try: + plane_model, inliers = pcd_down.segment_plane( + distance_threshold=0.015, ransac_n=3, num_iterations=1000 + ) + except Exception: + return + + if len(inliers) < 100: + return + + # Validate plane is roughly horizontal + normal = np.array(plane_model[:3]) + normal = normal / np.linalg.norm(normal) + angle_rad = np.arccos(np.clip(np.abs(normal.dot(up_vector)), -1.0, 1.0)) + if np.rad2deg(angle_rad) > 85.0: + self.get_logger().info(f"returned 9, angle was {np.rad2deg(angle_rad):.1f} deg") + return + + table_cloud = pcd_down.select_by_index(inliers) + table_pts = np.asarray(table_cloud.points) + + # Table centre + t_x = np.median(table_pts[:, 0]) + t_y = np.median(table_pts[:, 1]) + t_z = np.median(table_pts[:, 2]) + t_up = np.median(table_pts[:, up_idx]) # "height" of table surface + + # --- Floor estimation --- + if use_world_frame: + # In eddie_base_footprint, Z=0 is the ground plane by definition. + # Table height is simply the Z coordinate of the table surface. + floor_up_at_table = 0.0 + height_m = t_up + else: + # Camera frame: estimate floor via RANSAC on non-table points + pcd_full = o3d.geometry.PointCloud() + pcd_full.points = o3d.utility.Vector3dVector(work_points.astype(np.float64)) + pcd_full_down = pcd_full.voxel_down_sample(voxel_size=0.02) + all_pts = np.asarray(pcd_full_down.points) + table_plane_d = plane_model[3] + distances = np.abs(all_pts @ np.array(plane_model[:3]) + table_plane_d) + other_pts = all_pts[distances > 0.03] + + up_all = work_points[:, up_idx] + floor_seed = np.percentile(up_all, 90) if normal[up_idx] < 0 else np.percentile(up_all, 10) + floor_mask = (other_pts[:, up_idx] > t_up + 0.1) if normal[up_idx] < 0 \ + else (other_pts[:, up_idx] < t_up - 0.1) + + floor_candidates = other_pts[floor_mask] + floor_up_at_table = None + + if len(floor_candidates) > 50: + floor_candidates = floor_candidates[::max(1, len(floor_candidates) // 800)] + X_in = floor_candidates[:, [0, 2]] # X, Z → predict Y + Y_out = floor_candidates[:, 1] + pred_input = [[t_x, t_z]] + ransac = RANSACRegressor(residual_threshold=0.05) + try: + ransac.fit(X_in, Y_out) + floor_up_at_table = ransac.predict(pred_input)[0] + except Exception: + floor_up_at_table = None + + if floor_up_at_table is None: + floor_up_at_table = floor_seed + + height_m = abs(t_up - floor_up_at_table) + if height_m < 0.15 or height_m > 1.5: + self.get_logger().info(f"returned 10, height was {height_m:.3f}m") + return + + self.get_logger().info(f"Table height: {height_m:.3f}m ({'world' if use_world_frame else 'camera'} frame)") + + # Build marker header with correct frame + from copy import deepcopy + marker_header = deepcopy(pc_msg.header) + marker_header.frame_id = marker_frame + + self._publish_markers(t_x, t_y, t_z, t_up, floor_up_at_table, height_m, + marker_header, use_world_frame) + + debug_cloud = pc2.create_cloud_xyz32(marker_header, table_pts.astype(np.float32)) + self.debug_pc_pub.publish(debug_cloud) + + def _publish_markers(self, tx, ty, tz, t_up, floor_up, height_m, + header, use_world_frame): + marker_array = MarkerArray() + + # --- Table sphere (red) --- + m_table = Marker() + m_table.header = header + m_table.ns, m_table.id = "table", 0 + m_table.type = Marker.SPHERE + m_table.action = Marker.ADD + if use_world_frame: + m_table.pose.position.x = float(tx) + m_table.pose.position.y = float(ty) + m_table.pose.position.z = float(t_up) + else: + m_table.pose.position.x = float(tx) + m_table.pose.position.y = float(t_up) + m_table.pose.position.z = float(tz) + m_table.scale.x = m_table.scale.y = m_table.scale.z = 0.08 + m_table.color.r, m_table.color.g, m_table.color.b, m_table.color.a = 1.0, 0.0, 0.0, 1.0 + marker_array.markers.append(m_table) + + # --- Floor cube (green) --- + m_floor = Marker() + m_floor.header = header + m_floor.ns, m_floor.id = "floor", 1 + m_floor.type = Marker.CUBE + m_floor.action = Marker.ADD + if use_world_frame: + m_floor.pose.position.x = float(tx) + m_floor.pose.position.y = float(ty) + m_floor.pose.position.z = float(floor_up) + m_floor.scale.x, m_floor.scale.y, m_floor.scale.z = 0.3, 0.3, 0.005 + else: + m_floor.pose.position.x = float(tx) + m_floor.pose.position.y = float(floor_up) + m_floor.pose.position.z = float(tz) + m_floor.scale.x, m_floor.scale.z = 0.2, 0.2 + m_floor.scale.y = 0.005 + m_floor.color.r, m_floor.color.g, m_floor.color.b, m_floor.color.a = 0.0, 1.0, 0.0, 1.0 + marker_array.markers.append(m_floor) + + # --- Line (yellow) --- + m_line = Marker() + m_line.header = header + m_line.ns, m_line.id = "line", 2 + m_line.type = Marker.LINE_LIST + m_line.action = Marker.ADD + m_line.scale.x = 0.005 + m_line.color.r, m_line.color.g, m_line.color.b, m_line.color.a = 1.0, 1.0, 0.0, 1.0 + m_line.points.append(m_table.pose.position) + m_line.points.append(m_floor.pose.position) + marker_array.markers.append(m_line) + + # --- Text label (white) --- + m_text = Marker() + m_text.header = header + m_text.ns, m_text.id = "text", 3 + m_text.type = Marker.TEXT_VIEW_FACING + m_text.action = Marker.ADD + m_text.text = f"{height_m:.2f}m" + if use_world_frame: + m_text.pose.position.x = float(tx) + 0.15 + m_text.pose.position.y = float(ty) + m_text.pose.position.z = (t_up + floor_up) / 2.0 + else: + m_text.pose.position.x = float(tx) + 0.15 + m_text.pose.position.y = (t_up + floor_up) / 2.0 + m_text.pose.position.z = float(tz) + m_text.scale.z = 0.05 + m_text.color.r, m_text.color.g, m_text.color.b, m_text.color.a = 1.0, 1.0, 1.0, 1.0 + marker_array.markers.append(m_text) + + # Publish height value + height_msg = Float32() + height_msg.data = float(height_m) + if self.height_pub is not None: + self.height_pub.publish(height_msg) + + self.marker_pub.publish(marker_array) + + +def main(args=None): + rclpy.init(args=args) + node = TableHeightNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/r4s/src/perception/perception/brain_client.py b/r4s/src/perception/perception/brain_client.py new file mode 100644 index 0000000..4da4585 --- /dev/null +++ b/r4s/src/perception/perception/brain_client.py @@ -0,0 +1,119 @@ +import sys +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from action_msgs.msg import GoalStatus + +# Import your custom action +from my_robot_interfaces.action import RunVision + +class BrainClient(Node): + def __init__(self): + super().__init__('brain_client_node') + + # Create the action client + self._action_client = ActionClient(self, RunVision, 'run_perception_pipeline') + self.get_logger().info("Brain Client initialized. Ready to dispatch tasks.") + + def send_perception_task(self, task_name: str): + self.get_logger().info("Waiting for Perception Dispatcher to come online...") + + # Check if the server is available + if not self._action_client.wait_for_server(timeout_sec=10.0): + self.get_logger().error("Perception Dispatcher not found. Is the launch file running?") + return + + goal_msg = RunVision.Goal() + goal_msg.task_name = task_name + + self.get_logger().info(f"Sending goal request for task: '{task_name}'") + + # Send the goal and attach callbacks + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback + ) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def feedback_callback(self, feedback_msg): + """Prints live updates from the Dispatcher about what the AI is doing.""" + feedback = feedback_msg.feedback + self.get_logger().info(f"Dispatcher Feedback: [{feedback.current_phase}]") + + def goal_response_callback(self, future): + """Handles whether the Dispatcher accepted or rejected the task.""" + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().error('Task was REJECTED by the Perception Dispatcher.') + self._shutdown_client() + return + + self.get_logger().info('Task accepted! Waiting for pipeline execution to finish...') + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + """Processes the final outcome of the vision task.""" + status = future.result().status + result = future.result().result + + if status == GoalStatus.STATUS_SUCCEEDED: + if result.success: + self.get_logger().info("--------------------------------------------------") + self.get_logger().info("✅ VISION TASK SUCCESSFUL") + self.get_logger().info(f"RESULT: {result.message}") + self.get_logger().info("--------------------------------------------------") + + # If this was a placement task, you can access the pose directly if + # your action definition includes a pose/poses field. + # Example: if result.poses: ... + else: + self.get_logger().error(f"❌ Vision Task Failed: {result.message}") + else: + self.get_logger().error(f"🚨 Action Server Error. Status Code: {status}") + + self._shutdown_client() + + def _shutdown_client(self): + """Cleanly shuts down the node after task completion.""" + self.get_logger().info("Task complete. Shutting down Brain Client.") + # We use a timer to ensure the logs are printed before shutdown + self.create_timer(0.5, lambda: sys.exit(0)) + +def main(args=None): + # List of tasks your Dispatcher and Launch file are configured to handle + valid_tasks = [ + 'table_height', + 'detect_screws', + 'car_objects', + 'subdoor_pose', + 'detect_screwdriver', + 'place_object' # <--- NEW + ] + + # Simple CLI argument check + if len(sys.argv) < 2 or sys.argv[1] not in valid_tasks: + print("\nUsage: ros2 run perception brain_client ") + print(f"Available tasks: {', '.join(valid_tasks)}\n") + sys.exit(1) + + task_name = sys.argv[1] + + rclpy.init(args=args) + brain_client = BrainClient() + + # Start the task + brain_client.send_perception_task(task_name) + + try: + rclpy.spin(brain_client) + except KeyboardInterrupt: + print("\nBrain client stopped by user.") + finally: + brain_client.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/r4s/src/perception/perception/other/benchmark.py b/r4s/src/perception/perception/other/benchmark.py new file mode 100644 index 0000000..cf61458 --- /dev/null +++ b/r4s/src/perception/perception/other/benchmark.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +""" +Simple latency benchmark for perception package +Works with the two-node setup (yolo_node + pose_node) +""" + +import rclpy +from rclpy.node import Node +from vision_msgs.msg import Detection2DArray +from sensor_msgs.msg import Image +from geometry_msgs.msg import PoseStamped +import time +from collections import deque +import statistics + + +class SimpleBenchmark(Node): + + def __init__(self): + super().__init__('benchmark_node') + + # Storage for timing data + self.detections_received = deque(maxlen=300) + self.poses_received = deque(maxlen=300) + + # Latency tracking + self.detection_latencies = deque(maxlen=100) + self.pose_latencies = deque(maxlen=100) + self.e2e_latencies = deque(maxlen=100) + + # Track message timing + self.last_image_time = None + self.detection_count = 0 + self.pose_count = 0 + + self.get_logger().info("Subscribing to topics...") + + # Subscribe to raw image (for baseline timing) + self.image_sub = self.create_subscription( + Image, '/camera/color/image_raw', self.image_callback, 10) + + # Subscribe to detections + self.det_sub = self.create_subscription( + Detection2DArray, '/detections', self.detection_callback, 10) + + # Subscribe to poses (end-to-end latency) + self.pose_sub = self.create_subscription( + PoseStamped, '/object_pose', self.pose_callback, 10) + + # Subscribe to annotated image + self.annotated_sub = self.create_subscription( + Image, '/annotated_image', self.annotated_callback, 10) + + # Timer for reporting + self.timer = self.create_timer(5.0, self.report_stats) + + self.get_logger().info("Benchmark started. Listening for messages...") + + + def image_callback(self, msg): + """Track when raw image arrives.""" + self.last_image_time = time.time() + + + def detection_callback(self, msg): + """Track detections.""" + self.detection_count += 1 + self.detections_received.append(time.time()) + + if self.last_image_time and len(msg.detections) > 0: + latency = (time.time() - self.last_image_time) * 1000 + self.detection_latencies.append(latency) + + + def pose_callback(self, msg): + """Track poses (end-to-end latency).""" + self.pose_count += 1 + self.poses_received.append(time.time()) + + if self.last_image_time: + latency = (time.time() - self.last_image_time) * 1000 + self.e2e_latencies.append(latency) + + + def annotated_callback(self, msg): + """Just for confirmation node is running.""" + pass + + + def report_stats(self): + """Print statistics every 5 seconds.""" + + print("\n" + "="*70) + print("LATENCY BENCHMARK - PERCEPTION PACKAGE") + print("="*70) + + # Detection latency + if self.detection_latencies: + det_list = list(self.detection_latencies) + print("\n📊 DETECTION LATENCY (Image → Detection Output)") + print(f" Count: {len(det_list)} detections") + print(f" Mean: {statistics.mean(det_list):>8.2f} ms") + print(f" Median: {statistics.median(det_list):>8.2f} ms") + print(f" Min: {min(det_list):>8.2f} ms") + print(f" Max: {max(det_list):>8.2f} ms") + if len(det_list) > 1: + print(f" StdDev: {statistics.stdev(det_list):>8.2f} ms") + else: + print("\n⚠️ No detection data yet. Check if yolo_node is running.") + + # End-to-end latency + if self.e2e_latencies: + e2e_list = list(self.e2e_latencies) + print("\n🔍 END-TO-END LATENCY (Image → Pose Output)") + print(f" Count: {len(e2e_list)} poses") + print(f" Mean: {statistics.mean(e2e_list):>8.2f} ms") + print(f" Median: {statistics.median(e2e_list):>8.2f} ms") + print(f" Min: {min(e2e_list):>8.2f} ms") + print(f" Max: {max(e2e_list):>8.2f} ms") + if len(e2e_list) > 1: + print(f" StdDev: {statistics.stdev(e2e_list):>8.2f} ms") + else: + print("\n⚠️ No pose data yet. Check if pose_node is running.") + + # Throughput + if self.detections_received: + time_span = self.detections_received[-1] - self.detections_received[0] + if time_span > 0: + throughput = len(self.detections_received) / time_span + print(f"\n⚡ THROUGHPUT: {throughput:.1f} fps") + + print("="*70 + "\n") + + +def main(args=None): + rclpy.init(args=args) + benchmark = SimpleBenchmark() + + try: + rclpy.spin(benchmark) + except KeyboardInterrupt: + print("\n\nBenchmark stopped by user.") + finally: + benchmark.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/table_height_predictor/table_height_predictor/floor_detector_node.py b/r4s/src/perception/perception/other/floor_detector_node.py similarity index 100% rename from src/table_height_predictor/table_height_predictor/floor_detector_node.py rename to r4s/src/perception/perception/other/floor_detector_node.py diff --git a/r4s/src/perception/perception/other/obb_object_detection.py b/r4s/src/perception/perception/other/obb_object_detection.py new file mode 100644 index 0000000..28dd938 --- /dev/null +++ b/r4s/src/perception/perception/other/obb_object_detection.py @@ -0,0 +1,155 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose +from cv_bridge import CvBridge +from ultralytics import YOLO +import torch +import cv2 +import os +from ament_index_python.packages import get_package_share_directory +from pathlib import Path + +def get_package_name_from_path(file_path): + """Dynamically find the package name from the file path.""" + p = Path(file_path) + package_parts = p.parts[p.parts.index('site-packages') + 1:] + return package_parts[0] + +class OBBDetectorNode(Node): + def __init__(self): + self.package_name = get_package_name_from_path(__file__) + super().__init__('obb_detector_node') + self.bridge = CvBridge() + + # Declare and get parameters + self.declare_parameter('model_type', 'fine_tuned') # Using an OBB model + self.declare_parameter('input_mode', 'robot') + self.declare_parameter('model_path', '') + self.declare_parameter('conf_threshold', 0.6) + self.declare_parameter('device', '') + self.declare_parameter('class_names', []) + + model_type = self.get_parameter('model_type').get_parameter_value().string_value + input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + explicit_model_path = self.get_parameter('model_path').get_parameter_value().string_value + self.conf_threshold = self.get_parameter('conf_threshold').get_parameter_value().double_value + device_param = self.get_parameter('device').get_parameter_value().string_value + class_names_param = self.get_parameter('class_names').get_parameter_value().string_array_value + + # Determine model path + if explicit_model_path: + model_path = explicit_model_path + else: + package_share_directory = get_package_share_directory(self.package_name) + if model_type == 'fine_tuned': + model_path = os.path.join(package_share_directory, 'models', 'screwdriver_obb_best.pt') + else: + model_path = os.path.join(package_share_directory, 'models', 'yolov8n-obb.pt') + + self.get_logger().info(f"Using OBB model type '{model_type}' from: {model_path}") + + try: + self.model = YOLO(model_path) + if device_param: + try: + self.model.to(device_param) + self.get_logger().info(f"Model moved to device: {device_param}") + except Exception as e: + self.get_logger().warn(f"Failed to move model to device '{device_param}': {e}") + except Exception as e: + self.get_logger().error(f"Failed to load YOLO OBB model from '{model_path}': {e}") + raise + + self.class_names = None + if class_names_param: + self.class_names = list(class_names_param) + self.get_logger().info(f"Using class names from parameter (n={len(self.class_names)})") + + if input_mode == 'robot': + image_topic = '/camera/color/image_raw' + elif input_mode == 'realsense': + image_topic = '/camera/camera/color/image_raw' + else: + self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") + image_topic = '/camera/camera/color/image_raw' + + self.get_logger().info(f"Subscribing to image topic: {image_topic}") + + self.image_sub = self.create_subscription(Image, image_topic, self.image_callback, 10) + self.annotated_image_pub = self.create_publisher(Image, '/annotated_image', 10) + self.detection_pub = self.create_publisher(Detection2DArray, '/detections', 10) + + self.get_logger().info('YOLOv8-OBB Detector Node started.') + + def image_callback(self, msg): + try: + cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + except Exception as e: + self.get_logger().error(f'Image conversion error: {e}') + return + + # Run Inference + results = self.model(cv_image) + + detection_array_msg = Detection2DArray() + detection_array_msg.header = msg.header + + for result in results: + # Check for OBB results + if not hasattr(result, 'obb') or result.obb is None: + continue + + # Filter by confidence using boolean masking + mask = result.obb.conf >= self.conf_threshold + result.obb = result.obb[mask] + + # Publish Annotated Image (using original timestamp) + annotated_image = result.plot() + try: + annotated_msg = self.bridge.cv2_to_imgmsg(annotated_image, encoding='bgr8') + annotated_msg.header = msg.header + self.annotated_image_pub.publish(annotated_msg) + except Exception as e: + self.get_logger().error(f'Annotated image conversion error: {e}') + + # Fill Detection2D Messages + for obb in result.obb: + detection_msg = Detection2D() + detection_msg.header = msg.header + + hypothesis = ObjectHypothesisWithPose() + cls_id = int(obb.cls.item()) + + if self.class_names and cls_id < len(self.class_names): + class_name = self.class_names[cls_id] + else: + class_name = self.model.names.get(cls_id, str(cls_id)) + + hypothesis.hypothesis.class_id = class_name + hypothesis.hypothesis.score = float(obb.conf.item()) + detection_msg.results.append(hypothesis) + + # OBB Bounding Box (XYWHR) + xywhr = obb.xywhr.cpu().numpy().flatten() + + detection_msg.bbox.center.position.x = float(xywhr[0]) + detection_msg.bbox.center.position.y = float(xywhr[1]) + detection_msg.bbox.center.theta = float(xywhr[4]) # Capture the rotation! + + detection_msg.bbox.size_x = float(xywhr[2]) + detection_msg.bbox.size_y = float(xywhr[3]) + + detection_array_msg.detections.append(detection_msg) + + self.detection_pub.publish(detection_array_msg) + +def main(args=None): + rclpy.init(args=args) + node = OBBDetectorNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/perception/perception/pose_pca.py b/r4s/src/perception/perception/other/obb_pose_pca.py similarity index 65% rename from src/perception/perception/pose_pca.py rename to r4s/src/perception/perception/other/obb_pose_pca.py index ea0bbf0..e7a7115 100644 --- a/src/perception/perception/pose_pca.py +++ b/r4s/src/perception/perception/other/obb_pose_pca.py @@ -7,57 +7,57 @@ import message_filters from cv_bridge import CvBridge +import cv2 import numpy as np import struct import sensor_msgs_py.point_cloud2 as pc2 from transforms3d.quaternions import mat2quat from tf2_ros import TransformBroadcaster +from rclpy.qos import QoSProfile, ReliabilityPolicy -class PointCloudCropperNode(Node): +class PointOBBCloudCropperNode(Node): def __init__(self): - super().__init__('pointcloud_cropper_node') - + super().__init__('point_obb_cloud_cropper_node') self.bridge = CvBridge() - # Declare input_mode parameter only - self.declare_parameter('input_mode', 'realsense') + self.declare_parameter('input_mode', 'robot') input_mode = self.get_parameter('input_mode').get_parameter_value().string_value - # Determine topics based on mode if input_mode == 'robot': pc_topic = '/camera/depth/color/points' img_topic = '/camera/color/image_raw' - elif input_mode == 'realsense': - pc_topic = '/camera/camera/depth/color/points' - img_topic = '/camera/camera/color/image_raw' else: - self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") pc_topic = '/camera/camera/depth/color/points' img_topic = '/camera/camera/color/image_raw' - self.get_logger().info(f"Using input mode: '{input_mode}' with topics: {pc_topic}, {img_topic}") + # Match the rosbag's reliable QoS + reliable_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE) - # Message filter subscribers - pc_sub = message_filters.Subscriber(self, PointCloud2, pc_topic) - img_sub = message_filters.Subscriber(self, Image, img_topic) - det_sub = message_filters.Subscriber(self, Detection2DArray, '/detections') + self.pc_sub = message_filters.Subscriber(self, PointCloud2, pc_topic, qos_profile=reliable_qos) + self.img_sub = message_filters.Subscriber(self, Image, img_topic, qos_profile=reliable_qos) + self.det_sub = message_filters.Subscriber(self, Detection2DArray, '/detections', qos_profile=reliable_qos) - ts = message_filters.ApproximateTimeSynchronizer( - [pc_sub, det_sub, img_sub], - queue_size=10, - slop=0.1 + # Keep the relaxed synchronizer settings + self.ts = message_filters.ApproximateTimeSynchronizer( + [self.pc_sub, self.det_sub, self.img_sub], + queue_size=500, + slop=1.5 ) - ts.registerCallback(self.sync_callback) + self.ts.registerCallback(self.sync_callback) - # Publishers self.pc_pub = self.create_publisher(PointCloud2, '/cropped_pointcloud', 10) - self.pose_pub = self.create_publisher(PoseStamped, '/object_pose', 10) + self.pose_pub = self.create_publisher(PoseStamped, '/object_pose_screwdriver', 10) self.tf_broadcaster = TransformBroadcaster(self) - self.get_logger().info('PointCloud Cropper Node with PCA and TF broadcasting started.') + self.get_logger().info('PointCloud OBB Cropper Node started successfully.') def sync_callback(self, cloud_msg, detection_msg, image_msg): + + # If there are no detections, do nothing + if not detection_msg.detections: + return + try: color_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8') except Exception as e: @@ -76,37 +76,37 @@ def sync_callback(self, cloud_msg, detection_msg, image_msg): for idx, detection in enumerate(detection_msg.detections): detected_class = detection.results[0].hypothesis.class_id + cx = detection.bbox.center.position.x + cy = detection.bbox.center.position.y + theta = detection.bbox.center.theta + w = detection.bbox.size_x + h = detection.bbox.size_y - # No class filtering here anymore - - cx = int(detection.bbox.center.position.x) - cy = int(detection.bbox.center.position.y) - w = int(detection.bbox.size_x) - h = int(detection.bbox.size_y) + angle_deg = np.degrees(theta) + rect = ((cx, cy), (w, h), angle_deg) + box = cv2.boxPoints(rect) + box = np.int32(box) - xmin = max(cx - w // 2, 0) - xmax = min(cx + w // 2, pc_width) - ymin = max(cy - h // 2, 0) - ymax = min(cy + h // 2, pc_height) + mask = np.zeros((pc_height, pc_width), dtype=np.uint8) + cv2.fillPoly(mask, [box], 255) - cropped_points = cloud_points[ymin:ymax, xmin:xmax, :].reshape(-1, 3) - cropped_colors = color_image[ymin:ymax, xmin:xmax, :].reshape(-1, 3) + cropped_points = cloud_points[mask == 255] + cropped_colors = color_image[mask == 255] + # Drop invalid depth points (RealSense creates a lot of NaNs) valid_mask = ~np.isnan(cropped_points).any(axis=1) cropped_points = cropped_points[valid_mask] cropped_colors = cropped_colors[valid_mask] - for pt, color in zip(cropped_points, cropped_colors): - x, y, z = pt - b, g, r = color - rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] - all_colored_points.append([x, y, z, rgb]) - - self.get_logger().info( - f"Cropped '{detected_class}' object {idx}: [{xmin}:{xmax}, {ymin}:{ymax}] -> {len(cropped_points)} valid points" - ) + self.get_logger().info(f"Class '{detected_class}': Cropped {len(cropped_points)} valid 3D points.") if len(cropped_points) >= 3: + for pt, color in zip(cropped_points, cropped_colors): + x, y, z = pt + b, g, r = color + rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] + all_colored_points.append([x, y, z, rgb]) + centroid = np.mean(cropped_points, axis=0) centered = cropped_points - centroid _, _, vh = np.linalg.svd(centered, full_matrices=False) @@ -115,7 +115,6 @@ def sync_callback(self, cloud_msg, detection_msg, image_msg): T = np.eye(4) T[:3, :3] = R T[:3, 3] = centroid - quat_wxyz = mat2quat(T[:3, :3]) quat = [quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]] @@ -129,7 +128,6 @@ def sync_callback(self, cloud_msg, detection_msg, image_msg): pose_msg.pose.orientation.y = float(quat[1]) pose_msg.pose.orientation.z = float(quat[2]) pose_msg.pose.orientation.w = float(quat[3]) - self.pose_pub.publish(pose_msg) t = TransformStamped() @@ -143,33 +141,28 @@ def sync_callback(self, cloud_msg, detection_msg, image_msg): t.transform.rotation.y = float(quat[1]) t.transform.rotation.z = float(quat[2]) t.transform.rotation.w = float(quat[3]) - self.tf_broadcaster.sendTransform(t) - self.get_logger().info(f"Published pose and TF for '{detected_class}' object {idx}") if all_colored_points: header = Header() header.stamp = self.get_clock().now().to_msg() header.frame_id = cloud_msg.header.frame_id - fields = [ PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), ] - cropped_pc = pc2.create_cloud(header, fields, all_colored_points) self.pc_pub.publish(cropped_pc) - + self.get_logger().info(f"Published /cropped_pointcloud with total {len(all_colored_points)} points.") def main(args=None): rclpy.init(args=args) - node = PointCloudCropperNode() + node = PointOBBCloudCropperNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown() - if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/src/perception/perception/object_classifier_node.py b/r4s/src/perception/perception/other/object_classifier_node.py similarity index 100% rename from src/perception/perception/object_classifier_node.py rename to r4s/src/perception/perception/other/object_classifier_node.py diff --git a/src/perception/perception/segment_object.py b/r4s/src/perception/perception/other/segment_object.py similarity index 100% rename from src/perception/perception/segment_object.py rename to r4s/src/perception/perception/other/segment_object.py diff --git a/src/perception/perception/rviz/place_object.rviz b/r4s/src/perception/perception/rviz/place_object.rviz similarity index 100% rename from src/perception/perception/rviz/place_object.rviz rename to r4s/src/perception/perception/rviz/place_object.rviz diff --git a/Asjad/perception/rviz/pose_estimation.rviz b/r4s/src/perception/perception/rviz/pose_estimation.rviz similarity index 100% rename from Asjad/perception/rviz/pose_estimation.rviz rename to r4s/src/perception/perception/rviz/pose_estimation.rviz diff --git a/r4s/src/perception/perception/subdoor.py b/r4s/src/perception/perception/subdoor.py new file mode 100644 index 0000000..198cadc --- /dev/null +++ b/r4s/src/perception/perception/subdoor.py @@ -0,0 +1,187 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import Image, PointCloud2 +from geometry_msgs.msg import PoseStamped, Point +from visualization_msgs.msg import Marker, MarkerArray +from cv_bridge import CvBridge +import message_filters +import cv2 +import numpy as np +import struct +import math +import os +from ultralytics import YOLO +from ament_index_python.packages import get_package_share_directory +from pathlib import Path + +def get_package_name_from_path(file_path): + p = Path(file_path) + try: + idx = p.parts.index('site-packages') + return p.parts[idx + 1] + except (ValueError, IndexError): + return "perception" # Fallback to your package name + +class BodyPoseEstimatorNode(Node): + def __init__(self): + super().__init__('subdoor_pose_estimator') + self.package_name = get_package_name_from_path(__file__) + + # --- Parameters --- + self.declare_parameter('model_path', '') + self.declare_parameter('input_mode', 'robot') + self.declare_parameter('conf_threshold', 0.25) + + param_model_path = self.get_parameter('model_path').get_parameter_value().string_value + self.input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + self.conf = self.get_parameter('conf_threshold').get_parameter_value().double_value + + # Resolve Model Path + if param_model_path: + self.model_path = param_model_path + else: + share_dir = get_package_share_directory(self.package_name) + self.model_path = os.path.join(share_dir, 'models', 'subdoor.pt') + + # --- Setup YOLO --- + self.get_logger().info(f"Loading YOLO model from: {self.model_path}") + try: + self.model = YOLO(self.model_path) + except Exception as e: + self.get_logger().error(f"Failed to load model: {e}") + raise + + self.bridge = CvBridge() + + # --- Topics --- + img_topic = '/camera/color/image_raw' if self.input_mode == 'robot' else '/camera/camera/color/image_raw' + pc_topic = '/camera/depth/color/points' if self.input_mode == 'robot' else '/camera/camera/depth/color/points' + + # --- Synchronization --- + self.img_sub = message_filters.Subscriber(self, Image, img_topic, qos_profile=qos_profile_sensor_data) + self.pc_sub = message_filters.Subscriber(self, PointCloud2, pc_topic, qos_profile=qos_profile_sensor_data) + + self.ts = message_filters.ApproximateTimeSynchronizer([self.img_sub, self.pc_sub], queue_size=10, slop=0.5) + self.ts.registerCallback(self.sync_callback) + + # --- Publishers --- + self.marker_pub = self.create_publisher(MarkerArray, '/body_markers', 10) + self.debug_pub = self.create_publisher(Image, '/body_debug_image', 10) + + self.latest_poses = [] # Store the 4 corner points here + + self.get_logger().info("Subdoor Worker initialized and actively processing...") + + def get_xyz_from_cloud(self, cloud_msg, u, v, window=3): + """Samples a window around (u,v) to avoid NaNs at edges.""" + points = [] + # Search in a small window + for dy in range(-window, window + 1): + for dx in range(-window, window + 1): + curr_u, curr_v = u + dx, v + dy + if 0 <= curr_u < cloud_msg.width and 0 <= curr_v < cloud_msg.height: + offset = (curr_v * cloud_msg.row_step) + (curr_u * cloud_msg.point_step) + try: + x, y, z = struct.unpack_from('fff', cloud_msg.data, offset) + if not (math.isnan(x) or math.isnan(y) or math.isnan(z)): + points.append((x, y, z)) + except: continue + + if not points: + self.get_logger().warn(f"No valid depth found near pixel ({u}, {v})") + return None + # Return median to filter outliers/noise at the corner + return np.median(points, axis=0).tolist() + + def sync_callback(self, img_msg, pc_msg): + try: + cv_image = self.bridge.imgmsg_to_cv2(img_msg, 'bgr8') + except Exception as e: + self.get_logger().error(f"CV Bridge error: {e}") + return + + results = self.model(cv_image, conf=self.conf, verbose=False, retina_masks=True) + if not results or results[0].masks is None: + self.debug_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, 'bgr8')) + return + + target_contour = self.find_body_contour(results[0]) + if target_contour is not None: + four_points_2d = self.get_4_point_approx(target_contour) + if four_points_2d is not None: + points_3d = [] + for pt in four_points_2d: + u, v = pt[0] + xyz = self.get_xyz_from_cloud(pc_msg, u, v, window=5) + if xyz: points_3d.append(xyz) + + if len(points_3d) == 4: + self.latest_poses = points_3d + self.publish_results(points_3d, img_msg.header) + cv2.drawContours(cv_image, [four_points_2d], -1, (0, 255, 0), 2) + else: + self.get_logger().warn(f"3D extraction failed: {len(points_3d)}/4 points valid") + + self.debug_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, 'bgr8')) + + def find_body_contour(self, result): + body_id = next((i for i, name in result.names.items() if name == 'body'), None) + if body_id is None and len(result.names) == 1: + body_id = list(result.names.keys())[0] + + max_area, target_contour = 0, None + if result.boxes is not None: + for i, cls_tensor in enumerate(result.boxes.cls): + if int(cls_tensor) == body_id: + contour = result.masks.xy[i].astype(np.int32).reshape(-1, 1, 2) + area = cv2.contourArea(contour) + if area > max_area: + max_area, target_contour = area, contour + return target_contour + + def get_4_point_approx(self, contour): + epsilon_factor = 0.01 + for _ in range(50): + peri = cv2.arcLength(contour, True) + approx = cv2.approxPolyDP(contour, epsilon_factor * peri, True) + if len(approx) == 4: return approx + if len(approx) < 4: return None + epsilon_factor += 0.005 + return None + + def publish_results(self, points_3d, header): + pts = np.array(points_3d) + centroid = np.mean(pts, axis=0) + + # Simple SVD for orientation + centered = pts - centroid + _, _, vh = np.linalg.svd(centered) + R = vh.T + if np.linalg.det(R) < 0: R[:, 2] *= -1 + + try: + from transforms3d.quaternions import mat2quat + quat = mat2quat(R) + except ImportError: + quat = [1.0, 0.0, 0.0, 0.0] + + # Markers + ma = MarkerArray() + for i, pt in enumerate(points_3d): + m = Marker(header=header, ns="corners", id=i, type=Marker.SPHERE, action=Marker.ADD) + m.pose.position.x, m.pose.position.y, m.pose.position.z = map(float, pt) + m.scale.x = m.scale.y = m.scale.z = 0.05 + m.color.a, m.color.r = 1.0, 1.0 + ma.markers.append(m) + self.marker_pub.publish(ma) + +def main(args=None): + rclpy.init(args=args) + node = BodyPoseEstimatorNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/r4s/src/perception/perception/subdoor_pose_estimator.py b/r4s/src/perception/perception/subdoor_pose_estimator.py new file mode 100644 index 0000000..8f0c5b8 --- /dev/null +++ b/r4s/src/perception/perception/subdoor_pose_estimator.py @@ -0,0 +1,253 @@ +import rclpy +from rclpy.lifecycle import Node as LifecycleNode, State, TransitionCallbackReturn +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import Image, PointCloud2 +from visualization_msgs.msg import Marker, MarkerArray +from cv_bridge import CvBridge +import message_filters +import cv2 +import numpy as np +import struct +import math +import os +import torch +from ultralytics import YOLO +from ament_index_python.packages import get_package_share_directory +from pathlib import Path + +def get_package_name_from_path(file_path): + p = Path(file_path) + try: + idx = p.parts.index('site-packages') + return p.parts[idx + 1] + except (ValueError, IndexError): + return "perception" # Fallback to your package name + +class BodyPoseEstimatorLifecycleNode(LifecycleNode): + def __init__(self): + super().__init__('subdoor_pose_estimator') + self.package_name = get_package_name_from_path(__file__) + + # --- Parameters --- + self.declare_parameter('model_path', '') + self.declare_parameter('input_mode', 'robot') + self.declare_parameter('conf_threshold', 0.25) + + # Placeholders for resources + self.model = None + self.bridge = CvBridge() + self.model_path = "" + self.input_mode = "" + self.conf = 0.25 + + self.marker_pub = None + self.debug_pub = None + self.img_sub = None + self.pc_sub = None + self.ts = None + + self._is_active = False + self.latest_poses = [] + + self.get_logger().info("Body Pose Estimator Lifecycle Node Initialized (Unconfigured).") + + # --- LIFECYCLE CALLBACKS --- + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Configuring Body Pose Estimator Node...") + + try: + param_model_path = self.get_parameter('model_path').get_parameter_value().string_value + self.input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + self.conf = self.get_parameter('conf_threshold').get_parameter_value().double_value + + # Resolve Model Path + if param_model_path: + self.model_path = param_model_path + else: + share_dir = get_package_share_directory(self.package_name) + self.model_path = os.path.join(share_dir, 'models', 'subdoor.pt') + + # Load YOLO Model + self.get_logger().info(f"Loading YOLO model from: {self.model_path}") + self.model = YOLO(self.model_path) + + # --- Topics --- + img_topic = '/camera/color/image_raw' if self.input_mode == 'robot' else '/camera/camera/color/image_raw' + pc_topic = '/camera/depth/color/points' if self.input_mode == 'robot' else '/camera/camera/depth/color/points' + + # --- Lifecycle Publishers --- + self.marker_pub = self.create_lifecycle_publisher(MarkerArray, '/body_markers', 10) + self.debug_pub = self.create_lifecycle_publisher(Image, '/body_debug_image', 10) + + # --- Synchronization Subscriptions --- + self.img_sub = message_filters.Subscriber(self, Image, img_topic, qos_profile=qos_profile_sensor_data) + self.pc_sub = message_filters.Subscriber(self, PointCloud2, pc_topic, qos_profile=qos_profile_sensor_data) + + self.ts = message_filters.ApproximateTimeSynchronizer([self.img_sub, self.pc_sub], queue_size=10, slop=0.5) + self.ts.registerCallback(self.sync_callback) + + self.get_logger().info("Configuration complete.") + return TransitionCallbackReturn.SUCCESS + + except Exception as e: + self.get_logger().error(f"Failed to configure node: {e}") + return TransitionCallbackReturn.FAILURE + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Activating Body Pose Estimator Node...") + super().on_activate(state) + self._is_active = True + self.latest_poses = [] # Clear any stale data on fresh activation + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Deactivating Body Pose Estimator Node...") + self._is_active = False + super().on_deactivate(state) + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Cleaning up Body Pose Estimator resources...") + + self.destroy_publisher(self.marker_pub) + self.destroy_publisher(self.debug_pub) + + # Destroy underlying ROS subscriptions in message_filters + if self.img_sub is not None: + self.destroy_subscription(self.img_sub.sub) + if self.pc_sub is not None: + self.destroy_subscription(self.pc_sub.sub) + + self.marker_pub = self.debug_pub = None + self.img_sub = self.pc_sub = self.ts = None + + self.model = None + if torch.cuda.is_available(): + torch.cuda.empty_cache() + + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Shutting down Body Pose Estimator Node...") + if self.model is not None: + self.on_cleanup(state) + return TransitionCallbackReturn.SUCCESS + + # --- PROCESSING METHODS --- + + def get_xyz_from_cloud(self, cloud_msg, u, v, window=3): + """Samples a window around (u,v) to avoid NaNs at edges.""" + points = [] + for dy in range(-window, window + 1): + for dx in range(-window, window + 1): + curr_u, curr_v = u + dx, v + dy + if 0 <= curr_u < cloud_msg.width and 0 <= curr_v < cloud_msg.height: + offset = (curr_v * cloud_msg.row_step) + (curr_u * cloud_msg.point_step) + try: + x, y, z = struct.unpack_from('fff', cloud_msg.data, offset) + if not (math.isnan(x) or math.isnan(y) or math.isnan(z)): + points.append((x, y, z)) + except: continue + + if not points: + self.get_logger().warn(f"No valid depth found near pixel ({u}, {v})") + return None + return np.median(points, axis=0).tolist() + + def sync_callback(self, img_msg, pc_msg): + if not self._is_active: + return + + try: + cv_image = self.bridge.imgmsg_to_cv2(img_msg, 'bgr8') + except Exception as e: + self.get_logger().error(f"CV Bridge error: {e}") + return + + results = self.model(cv_image, conf=self.conf, verbose=False, retina_masks=True) + if not results or results[0].masks is None: + self.debug_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, 'bgr8')) + return + + target_contour = self.find_body_contour(results[0]) + if target_contour is not None: + four_points_2d = self.get_4_point_approx(target_contour) + if four_points_2d is not None: + points_3d = [] + for pt in four_points_2d: + u, v = pt[0] + xyz = self.get_xyz_from_cloud(pc_msg, u, v, window=5) + if xyz: points_3d.append(xyz) + + if len(points_3d) == 4: + self.latest_poses = points_3d + self.publish_results(points_3d, img_msg.header) + cv2.drawContours(cv_image, [four_points_2d], -1, (0, 255, 0), 2) + else: + self.get_logger().warn(f"3D extraction failed: {len(points_3d)}/4 points valid") + + self.debug_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, 'bgr8')) + + def find_body_contour(self, result): + body_id = next((i for i, name in result.names.items() if name == 'body'), None) + if body_id is None and len(result.names) == 1: + body_id = list(result.names.keys())[0] + + max_area, target_contour = 0, None + if result.boxes is not None: + for i, cls_tensor in enumerate(result.boxes.cls): + if int(cls_tensor) == body_id: + contour = result.masks.xy[i].astype(np.int32).reshape(-1, 1, 2) + area = cv2.contourArea(contour) + if area > max_area: + max_area, target_contour = area, contour + return target_contour + + def get_4_point_approx(self, contour): + epsilon_factor = 0.01 + for _ in range(50): + peri = cv2.arcLength(contour, True) + approx = cv2.approxPolyDP(contour, epsilon_factor * peri, True) + if len(approx) == 4: return approx + if len(approx) < 4: return None + epsilon_factor += 0.005 + return None + + def publish_results(self, points_3d, header): + pts = np.array(points_3d) + centroid = np.mean(pts, axis=0) + + centered = pts - centroid + _, _, vh = np.linalg.svd(centered) + R = vh.T + if np.linalg.det(R) < 0: R[:, 2] *= -1 + + try: + from transforms3d.quaternions import mat2quat + quat = mat2quat(R) + except ImportError: + quat = [1.0, 0.0, 0.0, 0.0] + + ma = MarkerArray() + for i, pt in enumerate(points_3d): + m = Marker(header=header, ns="corners", id=i, type=Marker.SPHERE, action=Marker.ADD) + m.pose.position.x, m.pose.position.y, m.pose.position.z = map(float, pt) + m.scale.x = m.scale.y = m.scale.z = 0.05 + m.color.a, m.color.r = 1.0, 1.0 + ma.markers.append(m) + self.marker_pub.publish(ma) + +def main(args=None): + rclpy.init(args=args) + node = BodyPoseEstimatorLifecycleNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/perception/perception/table_segmentation.py b/r4s/src/perception/perception/table_segmentation.py similarity index 68% rename from src/perception/perception/table_segmentation.py rename to r4s/src/perception/perception/table_segmentation.py index 4f08041..ce34085 100644 --- a/src/perception/perception/table_segmentation.py +++ b/r4s/src/perception/perception/table_segmentation.py @@ -1,22 +1,23 @@ import rclpy -from rclpy.node import Node +from rclpy.lifecycle import Node as LifecycleNode, State, TransitionCallbackReturn from sensor_msgs.msg import PointCloud2, PointField from geometry_msgs.msg import PoseStamped from visualization_msgs.msg import Marker -from std_msgs.msg import Float32, Header +from std_msgs.msg import Float32 import sensor_msgs_py.point_cloud2 as pc2 import open3d as o3d import numpy as np import struct -class TableSegmentationNode(Node): +from rclpy.signals import SignalHandlerOptions + +class TableSegmentationNode(LifecycleNode): def __init__(self): super().__init__('table_segmentation_node') - # Declare Parameters + # --- CONFIGURATION (Set in Unconfigured State) --- self.declare_parameter('input_mode', 'robot') self.declare_parameter('debug_viz', True) - self.declare_parameter('mode', 'test') # 'test' or 'actual' self.declare_parameter('test_radius', 0.07) # Default radius for test mode (7cm) self.declare_parameter('safety_margin', 0.02) # Default safety margin (2cm) @@ -31,43 +32,121 @@ def __init__(self): self.get_logger().info(f"Starting in MODE: {self.op_mode}") self.get_logger().info(f"Initial Radius: {self.current_radius}m | Safety Margin: {self.safety_margin}m") + # Determine topic if input_mode == 'robot': - pc_topic = '/camera/depth/color/points' + self.pc_topic = '/camera/depth/color/points' elif input_mode == 'realsense': - pc_topic = '/camera/camera/depth/color/points' + self.pc_topic = '/camera/camera/depth/color/points' else: - pc_topic = '/camera/depth/color/points' + self.pc_topic = '/camera/depth/color/points' - self.get_logger().info(f"Subscribing to: {pc_topic}") + # Placeholders for resources + self.pc_sub = None + self.car_radius_sub = None # <--- NEW + self.screw_radius_sub = None # <--- NEW + + self.place_pose_pub = None + self.viz_sphere_pub = None + self.table_cloud_pub = None + self.object_cloud_pub = None + self.viz_pub = None - self.pc_sub = self.create_subscription(PointCloud2, pc_topic, self.cloud_callback, 10) + self._is_active = False + self.get_logger().info("Table Segmentation Lifecycle Node Initialized (Unconfigured).") - # Radius Subscriber (Only active in actual mode) - if self.op_mode == 'actual': - self.radius_sub = self.create_subscription( - Float32, - '/perception/detected_object_radius', - self.radius_callback, - 10 - ) - self.get_logger().info("Listening to /perception/detected_object_radius for updates...") + # --- LIFECYCLE CALLBACKS --- + + def on_configure(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Configuring: Allocating publishers and subscribers...") + + try: + # 1. Create Lifecycle Publishers + self.place_pose_pub = self.create_lifecycle_publisher(PoseStamped, '/perception/target_place_pose', 10) + self.viz_sphere_pub = self.create_lifecycle_publisher(Marker, '/perception/debug/viz_sphere', 10) + self.table_cloud_pub = self.create_lifecycle_publisher(PointCloud2, '/perception/debug/table_plane', 10) + self.object_cloud_pub = self.create_lifecycle_publisher(PointCloud2, '/perception/debug/objects', 10) + self.viz_pub = self.create_lifecycle_publisher(Marker, '/perception/debug/viz_arrow', 10) + + # 2. Create Subscriptions + self.get_logger().info(f"Subscribing to: {self.pc_topic}") + self.pc_sub = self.create_subscription(PointCloud2, self.pc_topic, self.cloud_callback, 10) + + if self.op_mode == 'actual': + # Listen to BOTH pipelines! + self.car_radius_sub = self.create_subscription( + Float32, '/car/radius', self.radius_callback, 10 + ) + self.screw_radius_sub = self.create_subscription( + Float32, '/screw/radius', self.radius_callback, 10 + ) + self.get_logger().info("Listening to /car/radius and /screw/radius for updates...") + + self.get_logger().info("Configuration complete.") + return TransitionCallbackReturn.SUCCESS + except Exception as e: + self.get_logger().error(f"Failed to configure: {e}") + return TransitionCallbackReturn.FAILURE + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Activating node...") + super().on_activate(state) + self._is_active = True + self.get_logger().info("Node activated. Processing incoming point cloud data.") + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Deactivating node...") + self._is_active = False + super().on_deactivate(state) + self.get_logger().info("Node deactivated. Pausing processing.") + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Cleaning up resources...") + + # Destroy Publishers + self.destroy_publisher(self.place_pose_pub) + self.destroy_publisher(self.viz_sphere_pub) + self.destroy_publisher(self.table_cloud_pub) + self.destroy_publisher(self.object_cloud_pub) + self.destroy_publisher(self.viz_pub) + + # Destroy Subscriptions safely + if self.pc_sub is not None: + self.destroy_subscription(self.pc_sub) + if self.car_radius_sub is not None: + self.destroy_subscription(self.car_radius_sub) + if self.screw_radius_sub is not None: + self.destroy_subscription(self.screw_radius_sub) - # Publishers - self.place_pose_pub = self.create_publisher(PoseStamped, '/perception/target_place_pose', 10) - self.viz_sphere_pub = self.create_publisher(Marker, '/perception/debug/viz_sphere', 10) # Publisher for the Safety Sphere + # Reset placeholders + self.place_pose_pub = self.viz_sphere_pub = self.table_cloud_pub = self.object_cloud_pub = self.viz_pub = None + self.pc_sub = self.car_radius_sub = self.screw_radius_sub = None + + self.get_logger().info("Cleanup complete.") + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info("Shutting down node...") + if self.place_pose_pub is not None: + self.on_cleanup(state) + return TransitionCallbackReturn.SUCCESS + + # --- PROCESSING CALLBACKS --- - # Debug Publishers - self.table_cloud_pub = self.create_publisher(PointCloud2, '/perception/debug/table_plane', 10) - self.object_cloud_pub = self.create_publisher(PointCloud2, '/perception/debug/objects', 10) - self.viz_pub = self.create_publisher(Marker, '/perception/debug/viz_arrow', 10) # For RViz - Visual Offset above the table - def radius_callback(self, msg): - """Called only in actual mode when a new object size is detected.""" + if not self._is_active: + return + + # Updates the radius regardless of which topic published it if msg.data > 0.01: # Minimum 1cm radius self.current_radius = float(msg.data) self.get_logger().info(f"Updated Object Radius to: {self.current_radius:.3f}m") def cloud_callback(self, ros_cloud_msg): + if not self._is_active: + return + self.get_logger().info(f"Processing cloud: {ros_cloud_msg.width}x{ros_cloud_msg.height}") pcd = self.convert_ros_to_o3d(ros_cloud_msg) @@ -78,7 +157,6 @@ def cloud_callback(self, ros_cloud_msg): points = np.asarray(pcd.points) if len(points) > 0: - # Keep points where Z is less than max_depth mask = points[:, 2] < max_depth pcd = pcd.select_by_index(np.where(mask)[0]) @@ -90,8 +168,8 @@ def cloud_callback(self, ros_cloud_msg): try: plane_model, inliers = pcd_down.segment_plane(distance_threshold=0.008, - ransac_n=3, - num_iterations=1000) + ransac_n=3, + num_iterations=1000) except Exception as e: self.get_logger().warn("Could not find a plane (table) in view.") return @@ -99,7 +177,6 @@ def cloud_callback(self, ros_cloud_msg): table_cloud = pcd_down.select_by_index(inliers) raw_object_cloud = pcd_down.select_by_index(inliers, invert=True) - # Filter objects above the table object_cloud = self.filter_objects_above_table(raw_object_cloud, plane_model) total_check_radius = self.current_radius + self.safety_margin @@ -112,7 +189,6 @@ def cloud_callback(self, ros_cloud_msg): f"Target: X={target_point[0]:.2f}, Y={target_point[1]:.2f}, Z={target_point[2]:.2f}" ) - # Publish the real target pose for the robot real_pose = PoseStamped() real_pose.header = ros_cloud_msg.header real_pose.pose.position.x = float(target_point[0]) @@ -125,7 +201,6 @@ def cloud_callback(self, ros_cloud_msg): self.place_pose_pub.publish(real_pose) - # Publish a visualization pose slightly above the table for RViz viz_len = 0.15 hover_point = target_point - (arrow_vector * viz_len) @@ -137,20 +212,19 @@ def cloud_callback(self, ros_cloud_msg): viz_pose.pose.orientation = real_pose.pose.orientation self.publish_arrow_marker(viz_pose) - self.publish_safety_sphere(real_pose, self.current_radius) else: self.get_logger().warn("Table full or no safe spot found!") if self.debug_viz: - # Paint Green and Red table_cloud.paint_uniform_color([0, 1, 0]) object_cloud.paint_uniform_color([1, 0, 0]) - # Publish with Color support self.publish_o3d(table_cloud, self.table_cloud_pub, ros_cloud_msg.header) self.publish_o3d(object_cloud, self.object_cloud_pub, ros_cloud_msg.header) + # --- HELPER METHODS --- + def publish_safety_sphere(self, pose_stamped, radius): marker = Marker() marker.header = pose_stamped.header @@ -179,12 +253,11 @@ def publish_arrow_marker(self, pose_stamped): marker.id = 0 marker.type = Marker.ARROW marker.action = Marker.ADD - marker.pose = pose_stamped.pose - marker.scale.x = 0.15 # 15cm Long - marker.scale.y = 0.01 # 1cm Wide - marker.scale.z = 0.02 # 2cm Head + marker.scale.x = 0.15 + marker.scale.y = 0.01 + marker.scale.z = 0.02 marker.color.r = 0.0 marker.color.g = 1.0 @@ -194,27 +267,15 @@ def publish_arrow_marker(self, pose_stamped): self.viz_pub.publish(marker) def filter_objects_above_table(self, object_cloud, plane_model): - """ - Removes points that are on the wrong side of the table. - Also removes points that are too high to be relevant objects. - """ if len(object_cloud.points) == 0: return object_cloud a, b, c, d = plane_model - - # Determine camera side sign camera_sign = np.sign(d) - points = np.asarray(object_cloud.points) colors = np.asarray(object_cloud.colors) if object_cloud.has_colors() else None - # Calculate signed distance for all points distances = (points[:,0] * a) + (points[:,1] * b) + (points[:,2] * c) + d - - # Filter - # - Must be on the same side as camera (Above table) - # - Must be within 50cm of the table surface (Ignore ceiling/high noise) valid_mask = (np.sign(distances) == camera_sign) & (np.abs(distances) < 0.5) filtered_cloud = o3d.geometry.PointCloud() @@ -225,38 +286,27 @@ def filter_objects_above_table(self, object_cloud, plane_model): return filtered_cloud def get_orientation_from_plane(self, plane_model): - """ - Calculates a quaternion where the X-AXIS points into the table. - """ - # Get the Normal Vector [a,b,c] normal = np.array(plane_model[:3]) normal = normal / np.linalg.norm(normal) - # Define the arrow direction (X-axis) - if normal[1] < 0: # If Y is negative (pointing up) - target_x = -normal # Flip it to point down/table + if normal[1] < 0: + target_x = -normal else: target_x = normal - # Construct Orthogonal Axes ref_vector = np.array([0, 1, 0]) - # If too close to parallel, use World X (1,0,0) if np.abs(np.dot(target_x, ref_vector)) > 0.9: ref_vector = np.array([1, 0, 0]) - # Z = X cross Ref z_axis = np.cross(target_x, ref_vector) z_axis = z_axis / np.linalg.norm(z_axis) - # Y = Z cross X (Ensure orthogonality) y_axis = np.cross(z_axis, target_x) y_axis = y_axis / np.linalg.norm(y_axis) - # Create Rotation Matrix [ X Y Z ] R = np.array([target_x, y_axis, z_axis]).T - # Convert to Quaternion [x, y, z, w] tr = np.trace(R) if tr > 0: S = np.sqrt(tr + 1.0) * 2 @@ -290,12 +340,11 @@ def find_empty_spot(self, table_cloud, object_cloud, radius_to_check, plane_mode a, b, c, d = plane_model - # Get table geometry statistics table_pts = np.asarray(table_cloud.points) min_x, min_y = np.min(table_pts[:,0]), np.min(table_pts[:,1]) max_x, max_y = np.max(table_pts[:,0]), np.max(table_pts[:,1]) - avg_z = np.mean(table_pts[:,2]) # The height of the table plane + avg_z = np.mean(table_pts[:,2]) center_x, center_y = np.mean(table_pts[:,0]), np.mean(table_pts[:,1]) table_tree = o3d.geometry.KDTreeFlann(table_cloud) @@ -304,18 +353,13 @@ def find_empty_spot(self, table_cloud, object_cloud, radius_to_check, plane_mode object_tree = None if has_objects: - # Create a shadow cloud where all object points are projected onto the table plane (Z = avg_z). - # This ensures that tall objects block the grid points underneath them. obj_points = np.asarray(object_cloud.points).copy() - obj_points[:, 2] = avg_z # Force Z to match table height + obj_points[:, 2] = avg_z flat_object_cloud = o3d.geometry.PointCloud() flat_object_cloud.points = o3d.utility.Vector3dVector(obj_points) - - # Build tree on the flattened cloud object_tree = o3d.geometry.KDTreeFlann(flat_object_cloud) - # Grid search step = 0.05 best_pt = None best_score = -float('inf') @@ -323,44 +367,32 @@ def find_empty_spot(self, table_cloud, object_cloud, radius_to_check, plane_mode for x in np.arange(min_x, max_x, step): for y in np.arange(min_y , max_y, step): - if abs(c) < 0.001: # Avoid division by zero (vertical plane) + if abs(c) < 0.001: z_plane = avg_z else: z_plane = -(a*x + b*y + d) / c - # The candidate point follows the table tilt cand = np.array([x, y, z_plane]) - - # use a temp candidate at avg_z for the KDTree checks - # (the tree and objects are built around avg_z for simplicity) check_cand = np.array([x, y, avg_z]) - # Check if it is this actually on the table [k, _, _] = table_tree.search_radius_vector_3d(check_cand, 0.05) if k == 0: continue dist_center = np.sqrt((x - center_x)**2 + (y - center_y)**2) - # Check collision with flattened objects if has_objects: - # Finds distance to the nearest object shadow [_, _, d_sq] = object_tree.search_knn_vector_3d(check_cand, 1) dist_obj = np.sqrt(d_sq[0]) - # Radius Check if dist_obj < radius_to_check: continue - # Score: Maximize distance to object, minimize distance to center score = dist_obj - (0.8 * dist_center) else: score = -dist_center - # Check if object radius fits on table - if (x - radius_to_check < min_x) or (x + radius_to_check > max_x): - continue - if (y - radius_to_check < min_y) or (y + radius_to_check > max_y): - continue + if (x - radius_to_check < min_x) or (x + radius_to_check > max_x): continue + if (y - radius_to_check < min_y) or (y + radius_to_check > max_y): continue if score > best_score: best_score = score @@ -390,13 +422,11 @@ def publish_o3d(self, o3d_cloud, publisher, header): if len(points) == 0: return - # If no colors, just send XYZ if not o3d_cloud.has_colors(): msg = pc2.create_cloud_xyz32(header, points) publisher.publish(msg) return - # Pack RGB color into the message colors = np.asarray(o3d_cloud.colors) points_with_color = [] @@ -417,12 +447,18 @@ def publish_o3d(self, o3d_cloud, publisher, header): msg = pc2.create_cloud(header, fields, points_with_color) publisher.publish(msg) + def main(args=None): - rclpy.init(args=args) + rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) node = TableSegmentationNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/src/perception/perception/yolo_object_detection.py b/r4s/src/perception/perception/yolo_object_detection.py similarity index 73% rename from src/perception/perception/yolo_object_detection.py rename to r4s/src/perception/perception/yolo_object_detection.py index 171e569..7f01a77 100644 --- a/src/perception/perception/yolo_object_detection.py +++ b/r4s/src/perception/perception/yolo_object_detection.py @@ -10,6 +10,7 @@ import os from ament_index_python.packages import get_package_share_directory from pathlib import Path +from std_msgs.msg import Float32 def get_package_name_from_path(file_path): """Dynamically find the package name from the file path.""" @@ -29,8 +30,8 @@ def __init__(self): self.bridge = CvBridge() # Declare and get parameters - self.declare_parameter('model_type', 'default') # 'default' or 'fine_tuned' - self.declare_parameter('input_mode', 'realsense') # 'robot' or 'realsense' + self.declare_parameter('model_type', 'fine_tuned') # 'default' or 'fine_tuned' + self.declare_parameter('input_mode', 'robot') # 'robot' or 'realsense' self.declare_parameter('model_path', '') # Optional explicit path to .pt self.declare_parameter('conf_threshold', 0.6) # Confidence threshold for filtering self.declare_parameter('device', '') # e.g., 'cpu', 'cuda:0' (optional) @@ -49,14 +50,16 @@ def __init__(self): else: # Get the absolute path to the package's share directory package_share_directory = get_package_share_directory(self.package_name) - - if model_type == 'fine_tuned': - # Construct the path to the model relative to the share directory - model_path = os.path.join(package_share_directory, 'models', 'fine_tuned.pt') - else: - model_path = os.path.join(package_share_directory, 'models', 'yolov8n.pt') - self.get_logger().info(f"Using model type '{model_type}' from: {model_path}") + model_map = { + 'fine_tuned': 'fine_tuned.pt', + 'screw': 'screw_best.pt', + 'default': 'yolov8n.pt' + } + + model_filename = model_map.get(model_type, 'yolov8n.pt') + model_path = os.path.join(package_share_directory, 'models', model_filename) + self.get_logger().info(f"Using model type '{model_type}' from: {model_path}") try: if device_param: @@ -91,35 +94,46 @@ def __init__(self): self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") image_topic = '/camera/camera/color/image_raw' - self.get_logger().info(f"Subscribing to image topic: {image_topic}") + self.get_logger().info(f"Using input_mode '{input_mode}' Subscribing to image topic: {image_topic}") # Create subscriptions and publishers self.image_sub = self.create_subscription(Image, image_topic, self.image_callback, 10) self.annotated_image_pub = self.create_publisher(Image, '/annotated_image', 10) self.detection_pub = self.create_publisher(Detection2DArray, '/detections', 10) + self.max_dim_pub = self.create_publisher(Float32, '/detections/max_dimension', 10) self.get_logger().info('YOLOv8 Detector Node started.') def image_callback(self, msg): try: + # 1. Convert Image cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') except Exception as e: self.get_logger().error(f'Image conversion error: {e}') return + # 2. Run Inference results = self.model(cv_image) + + # 3. Initialize Message with ORIGINAL Timestamp + # This is critical for the PointCloudCropper's Synchronizer detection_array_msg = Detection2DArray() - detection_array_msg.header = msg.header + detection_array_msg.header = msg.header + + current_frame_max_dim = 0.0 for result in results: + # Filter by confidence filtered_boxes = [box for box in result.boxes if float(box.conf) >= self.conf_threshold] + # Update boxes for result.plot() if filtered_boxes: box_data = torch.stack([b.data[0] for b in filtered_boxes]) result.boxes = Boxes(box_data, orig_shape=result.orig_shape) else: result.boxes = Boxes(torch.empty((0, 6)), orig_shape=result.orig_shape) + # Publish Annotated Image (using original timestamp) annotated_image = result.plot() try: annotated_msg = self.bridge.cv2_to_imgmsg(annotated_image, encoding='bgr8') @@ -128,32 +142,47 @@ def image_callback(self, msg): except Exception as e: self.get_logger().error(f'Annotated image conversion error: {e}') + # 4. Fill Detection2D Messages for box in filtered_boxes: detection_msg = Detection2D() - detection_msg.header = msg.header + detection_msg.header = msg.header # Consistency across sub-messages hypothesis = ObjectHypothesisWithPose() - try: - if self.class_names and int(box.cls) < len(self.class_names): - class_name = self.class_names[int(box.cls)] - else: - class_name = self.model.names[int(box.cls)] - except Exception: - class_name = str(int(box.cls)) + cls_id = int(box.cls) + + # Safe class name lookup + if self.class_names and cls_id < len(self.class_names): + class_name = self.class_names[cls_id] + else: + class_name = self.model.names.get(cls_id, str(cls_id)) + hypothesis.hypothesis.class_id = class_name hypothesis.hypothesis.score = float(box.conf) detection_msg.results.append(hypothesis) + # Bounding Box (XYWH) xywh = box.xywh.cpu().numpy().flatten() + w = float(xywh[2]) # Define w + h = float(xywh[3]) # Define h detection_msg.bbox.center.position.x = float(xywh[0]) detection_msg.bbox.center.position.y = float(xywh[1]) detection_msg.bbox.size_x = float(xywh[2]) detection_msg.bbox.size_y = float(xywh[3]) + # Track the largest dimension (width or height) + max_dim = max(w, h) + if max_dim > current_frame_max_dim: + current_frame_max_dim = max_dim + detection_array_msg.detections.append(detection_msg) + # 5. Single Publish per Image Frame self.detection_pub.publish(detection_array_msg) + if detection_array_msg.detections: + dim_msg = Float32() + dim_msg.data = current_frame_max_dim + self.max_dim_pub.publish(dim_msg) def main(args=None): rclpy.init(args=args) diff --git a/r4s/src/perception/perception/yolo_pose.py b/r4s/src/perception/perception/yolo_pose.py new file mode 100644 index 0000000..2626e09 --- /dev/null +++ b/r4s/src/perception/perception/yolo_pose.py @@ -0,0 +1,428 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2, PointField, Image +from vision_msgs.msg import Detection2DArray +from geometry_msgs.msg import PoseStamped, TransformStamped +from std_msgs.msg import Header +from visualization_msgs.msg import Marker +from std_msgs.msg import Float32 + + +import message_filters +from cv_bridge import CvBridge +import numpy as np +import struct +import sensor_msgs_py.point_cloud2 as pc2 +from transforms3d.quaternions import mat2quat +from tf2_ros import TransformBroadcaster + +# IMPORT QOS PROFILES +from rclpy.qos import qos_profile_sensor_data, QoSProfile, ReliabilityPolicy + +class PointCloudCropperNode(Node): + def __init__(self): + super().__init__('pointcloud_cropper_node') + self.bridge = CvBridge() + + # ... (Parameter declarations remain the same) ... + self.declare_parameter('input_mode', 'robot') + input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + + if input_mode == 'robot': + pc_topic = '/camera/depth/color/points' + img_topic = '/camera/color/image_raw' + else: + pc_topic = '/camera/camera/depth/color/points' + img_topic = '/camera/camera/color/image_raw' + + self.get_logger().info(f"Using topics: {pc_topic}, {img_topic}") + + # --- THE QOS FIX --- + # Camera data usually requires Sensor Data QoS (Best Effort) + self.pc_sub = message_filters.Subscriber( + self, PointCloud2, pc_topic, qos_profile=qos_profile_sensor_data) + + self.img_sub = message_filters.Subscriber( + self, Image, img_topic, qos_profile=qos_profile_sensor_data) + + # Detections are published by your YOLO node with default (Reliable) QoS + # We explicitly define a reliable profile here to match it. + reliable_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE) + self.det_sub = message_filters.Subscriber( + self, Detection2DArray, '/detections', qos_profile=reliable_qos) + + # --- THE SYNCHRONIZER FIX --- + # Queue size of 100 ensures the node holds onto 3 seconds of point clouds (at 30fps) + # while waiting for the YOLO node to finish processing the image. + self.ts = message_filters.ApproximateTimeSynchronizer( + [self.pc_sub, self.det_sub, self.img_sub], + queue_size=100, + slop=0.05 # 50ms slop is plenty since YOLO passes the original timestamp + ) + self.ts.registerCallback(self.sync_callback) + + # Publishers + self.pc_pub = self.create_publisher(PointCloud2, '/cropped_pointcloud', 10) + self.pose_pub = self.create_publisher(PoseStamped, '/object_pose', 10) + self.marker_pub = self.create_publisher(Marker, '/object_sphere_marker', 10) # NEW PUBLISHER + self.radius_pub = self.create_publisher(Float32, '/perception/detected_object_radius', 10) + self.tf_broadcaster = TransformBroadcaster(self) + + self.get_logger().info('PointCloud Cropper Node started (Waiting for Synced Data...).') + + def sync_callback(self, cloud_msg, detection_msg, image_msg): + try: + color_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8') + except Exception as e: + self.get_logger().error(f"Image conversion error: {e}") + return + + pc_width = cloud_msg.width + pc_height = cloud_msg.height + + cloud_points = np.array([ + [x, y, z] + for x, y, z in pc2.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=False) + ]).reshape((pc_height, pc_width, 3)) + + all_colored_points = [] + + for idx, detection in enumerate(detection_msg.detections): + w_2d = detection.bbox.size_x + h_2d = detection.bbox.size_y + max_dist_2d = max(w_2d, h_2d) + + detected_class = detection.results[0].hypothesis.class_id + + # No class filtering here anymore + + cx = int(detection.bbox.center.position.x) + cy = int(detection.bbox.center.position.y) + w = int(detection.bbox.size_x) + h = int(detection.bbox.size_y) + + xmin = max(cx - w // 2, 0) + xmax = min(cx + w // 2, pc_width) + ymin = max(cy - h // 2, 0) + ymax = min(cy + h // 2, pc_height) + + cropped_points = cloud_points[ymin:ymax, xmin:xmax, :].reshape(-1, 3) + cropped_colors = color_image[ymin:ymax, xmin:xmax, :].reshape(-1, 3) + + valid_mask = ~np.isnan(cropped_points).any(axis=1) + cropped_points = cropped_points[valid_mask] + cropped_colors = cropped_colors[valid_mask] + + for pt, color in zip(cropped_points, cropped_colors): + x, y, z = pt + b, g, r = color + rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] + all_colored_points.append([x, y, z, rgb]) + + self.get_logger().info( + f"Cropped '{detected_class}' object {idx}: [{xmin}:{xmax}, {ymin}:{ymax}] -> {len(cropped_points)} valid points" + ) + + if len(cropped_points) >= 3: + centroid = np.mean(cropped_points, axis=0) + centered = cropped_points - centroid + _, _, vh = np.linalg.svd(centered, full_matrices=False) + R = vh.T + + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = centroid + + quat_wxyz = mat2quat(T[:3, :3]) + quat = [quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]] + + pose_msg = PoseStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = cloud_msg.header.frame_id + pose_msg.pose.position.x = float(centroid[0]) + pose_msg.pose.position.y = float(centroid[1]) + pose_msg.pose.position.z = float(centroid[2]) + pose_msg.pose.orientation.x = float(quat[0]) + pose_msg.pose.orientation.y = float(quat[1]) + pose_msg.pose.orientation.z = float(quat[2]) + pose_msg.pose.orientation.w = float(quat[3]) + + self.pose_pub.publish(pose_msg) + + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = cloud_msg.header.frame_id + t.child_frame_id = f'object_frame_{idx}' + t.transform.translation.x = float(centroid[0]) + t.transform.translation.y = float(centroid[1]) + t.transform.translation.z = float(centroid[2]) + t.transform.rotation.x = float(quat[0]) + t.transform.rotation.y = float(quat[1]) + t.transform.rotation.z = float(quat[2]) + t.transform.rotation.w = float(quat[3]) + + self.tf_broadcaster.sendTransform(t) + self.get_logger().info(f"Published pose and TF for '{detected_class}' object {idx}") + + # --- SPHERE MARKER LOGIC --- + # 1. Calculate the REAL 3D diameter using the cropped point cloud + # This guarantees the sphere is measured in actual meters, not pixels! + distances = np.linalg.norm(cropped_points - centroid, axis=1) + radius = np.max(distances) + + radius_msg = Float32() + metric_radius = float(np.percentile(distances, 98)) + radius_msg.data = metric_radius + self.radius_pub.publish(radius_msg) + + sphere_diameter = float(metric_radius * 2.0) + + self.get_logger().info(f"Published METRIC radius: {metric_radius:.4f}m") + + # 2. Create the Marker + marker = Marker() + marker.header = cloud_msg.header + marker.ns = "object_spheres" + marker.id = idx + marker.type = Marker.SPHERE + marker.action = Marker.ADD + + # 3. Set Position + marker.pose.position.x = float(centroid[0]) + marker.pose.position.y = float(centroid[1]) + marker.pose.position.z = float(centroid[2]) + + # CRITICAL FIX: RViz needs a valid quaternion, otherwise it might hide the marker + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + + # 4. Set Scale to our calculated 3D diameter + marker.scale.x = sphere_diameter + marker.scale.y = sphere_diameter + marker.scale.z = sphere_diameter + + # 5. Set Color (Green, semi-transparent) + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 0.5 + + self.marker_pub.publish(marker) + + if all_colored_points: + header = Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = cloud_msg.header.frame_id + + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), + ] + + cropped_pc = pc2.create_cloud(header, fields, all_colored_points) + self.pc_pub.publish(cropped_pc) + + +def main(args=None): + rclpy.init(args=args) + node = PointCloudCropperNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + + +#### FOR CAR_OBJECT_DETECTION_NODE +# from rclpy.node import Node +# from sensor_msgs.msg import PointCloud2, PointField, Image +# from vision_msgs.msg import Detection2DArray +# from geometry_msgs.msg import PoseStamped, TransformStamped +# from std_msgs.msg import Header + +# import message_filters +# from cv_bridge import CvBridge +# import numpy as np +# import struct +# import sensor_msgs_py.point_cloud2 as pc2 +# from transforms3d.quaternions import mat2quat +# from tf2_ros import TransformBroadcaster + +# # IMPORT QOS PROFILES +# from rclpy.qos import qos_profile_sensor_data, QoSProfile, ReliabilityPolicy + +# class PointCloudCropperNode(Node): +# def __init__(self): +# super().__init__('pointcloud_cropper_node') +# self.bridge = CvBridge() + +# # Storage for the single-shot detection +# self.latest_detections = None + +# # Fixed parameter default to match your YOLO node +# self.declare_parameter('input_mode', 'robot') +# input_mode = self.get_parameter('input_mode').get_parameter_value().string_value + +# if input_mode == 'robot': +# pc_topic = '/camera/depth/color/points' +# img_topic = '/camera/color/image_raw' +# else: +# pc_topic = '/camera/camera/depth/color/points' +# img_topic = '/camera/camera/color/image_raw' + +# self.get_logger().info(f"Using topics: {pc_topic}, {img_topic}") + +# # 1. Separate Subscriber for Detections (RELIABLE QoS) +# reliable_qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE) +# self.det_sub = self.create_subscription( +# Detection2DArray, '/detections', self.detection_callback, reliable_qos) + +# # 2. Time Synchronizer ONLY for the live camera feeds (SENSOR DATA QoS) +# self.pc_sub = message_filters.Subscriber( +# self, PointCloud2, pc_topic, qos_profile=qos_profile_sensor_data) + +# self.img_sub = message_filters.Subscriber( +# self, Image, img_topic, qos_profile=qos_profile_sensor_data) + +# # Loosened slop to 0.2 to allow for depth/rgb lag +# self.ts = message_filters.ApproximateTimeSynchronizer( +# [self.pc_sub, self.img_sub], queue_size=30, slop=0.2 +# ) +# self.ts.registerCallback(self.sync_callback) + +# # Publishers +# self.pc_pub = self.create_publisher(PointCloud2, '/cropped_pointcloud', 10) +# self.pose_pub = self.create_publisher(PoseStamped, '/object_pose', 10) +# self.tf_broadcaster = TransformBroadcaster(self) + +# self.get_logger().info('PointCloud Cropper Node started (Waiting for single YOLO detection...).') + +# def detection_callback(self, msg): +# # Save the detections to memory +# self.latest_detections = msg.detections +# self.get_logger().info(f"Received {len(self.latest_detections)} detections! Cropper is now active.") + +# # NOTICE: detection_msg has been completely removed from this signature! +# def sync_callback(self, cloud_msg, image_msg): +# # Guard check: Don't do anything until detections are received +# if not self.latest_detections: +# return + +# try: +# color_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8') +# except Exception as e: +# self.get_logger().error(f"Image conversion error: {e}") +# return + +# pc_width = cloud_msg.width +# pc_height = cloud_msg.height + +# cloud_points = np.array([ +# [x, y, z] +# for x, y, z in pc2.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=False) +# ]).reshape((pc_height, pc_width, 3)) + +# all_colored_points = [] + +# # Iterate over the SAVED detections +# for idx, detection in enumerate(self.latest_detections): +# detected_class = detection.results[0].hypothesis.class_id + +# cx = int(detection.bbox.center.position.x) +# cy = int(detection.bbox.center.position.y) +# w = int(detection.bbox.size_x) +# h = int(detection.bbox.size_y) + +# xmin = max(cx - w // 2, 0) +# xmax = min(cx + w // 2, pc_width) +# ymin = max(cy - h // 2, 0) +# ymax = min(cy + h // 2, pc_height) + +# cropped_points = cloud_points[ymin:ymax, xmin:xmax, :].reshape(-1, 3) +# cropped_colors = color_image[ymin:ymax, xmin:xmax, :].reshape(-1, 3) + +# valid_mask = ~np.isnan(cropped_points).any(axis=1) +# cropped_points = cropped_points[valid_mask] +# cropped_colors = cropped_colors[valid_mask] + +# for pt, color in zip(cropped_points, cropped_colors): +# x, y, z = pt +# b, g, r = color +# rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] +# all_colored_points.append([x, y, z, rgb]) + +# # Print out the results to terminal so you know it's working +# self.get_logger().info( +# f"Cropped '{detected_class}' object {idx}: [{xmin}:{xmax}, {ymin}:{ymax}] -> {len(cropped_points)} valid points" +# ) + +# if len(cropped_points) >= 3: +# centroid = np.mean(cropped_points, axis=0) +# centered = cropped_points - centroid +# _, _, vh = np.linalg.svd(centered, full_matrices=False) +# R = vh.T + +# T = np.eye(4) +# T[:3, :3] = R +# T[:3, 3] = centroid + +# quat_wxyz = mat2quat(T[:3, :3]) +# quat = [quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]] + +# pose_msg = PoseStamped() +# pose_msg.header.stamp = self.get_clock().now().to_msg() +# pose_msg.header.frame_id = cloud_msg.header.frame_id +# pose_msg.pose.position.x = float(centroid[0]) +# pose_msg.pose.position.y = float(centroid[1]) +# pose_msg.pose.position.z = float(centroid[2]) +# pose_msg.pose.orientation.x = float(quat[0]) +# pose_msg.pose.orientation.y = float(quat[1]) +# pose_msg.pose.orientation.z = float(quat[2]) +# pose_msg.pose.orientation.w = float(quat[3]) + +# self.pose_pub.publish(pose_msg) + +# t = TransformStamped() +# t.header.stamp = self.get_clock().now().to_msg() +# t.header.frame_id = cloud_msg.header.frame_id +# t.child_frame_id = f'object_frame_{idx}' +# t.transform.translation.x = float(centroid[0]) +# t.transform.translation.y = float(centroid[1]) +# t.transform.translation.z = float(centroid[2]) +# t.transform.rotation.x = float(quat[0]) +# t.transform.rotation.y = float(quat[1]) +# t.transform.rotation.z = float(quat[2]) +# t.transform.rotation.w = float(quat[3]) + +# self.tf_broadcaster.sendTransform(t) + +# if all_colored_points: +# header = Header() +# header.stamp = self.get_clock().now().to_msg() +# header.frame_id = cloud_msg.header.frame_id + +# fields = [ +# PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), +# PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), +# PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), +# PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), +# ] + +# cropped_pc = pc2.create_cloud(header, fields, all_colored_points) +# self.pc_pub.publish(cropped_pc) + +# def main(args=None): +# rclpy.init(args=args) +# node = PointCloudCropperNode() +# rclpy.spin(node) +# node.destroy_node() +# rclpy.shutdown() + +# if __name__ == '__main__': +# main() \ No newline at end of file diff --git a/Asjad/perception/resource/perception b/r4s/src/perception/resource/perception similarity index 100% rename from Asjad/perception/resource/perception rename to r4s/src/perception/resource/perception diff --git a/Asjad/perception/setup.cfg b/r4s/src/perception/setup.cfg similarity index 100% rename from Asjad/perception/setup.cfg rename to r4s/src/perception/setup.cfg diff --git a/src/perception/setup.py b/r4s/src/perception/setup.py similarity index 52% rename from src/perception/setup.py rename to r4s/src/perception/setup.py index fc8bb06..282dc73 100644 --- a/src/perception/setup.py +++ b/r4s/src/perception/setup.py @@ -19,7 +19,13 @@ # Install launch files (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), ], - install_requires=['setuptools'], + install_requires=[ + 'setuptools', + 'open3d', + 'scikit-learn', + 'numpy', + 'opencv-python' + ], zip_safe=True, maintainer='mohsin', maintainer_email='mohsinalimirxa@gmail.com', @@ -27,16 +33,35 @@ license='TODO: License declaration', entry_points={ 'console_scripts': [ - "yolo_node = perception.yolo_object_detection:main", - "classifier_node = perception.object_classifier_node:main", - "pose_node = perception.pose_pca:main", "action_pose_node = perception.action_pose_pca:main", "action_yolo_node = perception.action_yolo_object_detection:main", "vision_manager_node = perception.action_vision_manager:main", + "brain_client = perception.brain_client:main", + + # 1 + "yolo_node = perception.yolo_object_detection:main", + "pose_node = perception.yolo_pose:main", + + #2 Subdoor + "subdoor_node = perception.subdoor_pose_estimator:main", + "subdoor = perception.subdoor:main", + + #3 Table Height + "table_height = perception.table_height_node:main", + + #4 Place Object "table_segmentation_node = perception.table_segmentation:main", - "segment_object = perception.segment_object:main", - + #5 Screwdriver + "obb_node = perception.action_obb_object_detection:main", + "pose_obb_node = perception.action_obb_pose:main", + # "obb_node = perception.obb_object_detection:main", + # "pose_obb_node = perception.obb_pose_pca:main", + + # "segment_object = perception.segment_object:main", + # "benchmark_node = perception.benchmark:main", + # "detect_floor = perception.floor_detector_node:main", + # "classifier_node = perception.object_classifier_node:main", ], }, diff --git a/src/perception/test/test_copyright.py b/r4s/src/perception/test/test_copyright.py similarity index 100% rename from src/perception/test/test_copyright.py rename to r4s/src/perception/test/test_copyright.py diff --git a/src/perception/test/test_flake8.py b/r4s/src/perception/test/test_flake8.py similarity index 100% rename from src/perception/test/test_flake8.py rename to r4s/src/perception/test/test_flake8.py diff --git a/src/perception/test/test_pep257.py b/r4s/src/perception/test/test_pep257.py similarity index 100% rename from src/perception/test/test_pep257.py rename to r4s/src/perception/test/test_pep257.py diff --git a/results/object_segmentation.png b/results/object_segmentation.png deleted file mode 100644 index 97a5661..0000000 Binary files a/results/object_segmentation.png and /dev/null differ diff --git a/src/my_robot_interfaces/action/RunVision.action b/src/my_robot_interfaces/action/RunVision.action deleted file mode 100644 index a475110..0000000 --- a/src/my_robot_interfaces/action/RunVision.action +++ /dev/null @@ -1,11 +0,0 @@ -# Goal: What we send to start the task -float32 duration_seconds ---- -# Result: What we get back when finished/canceled -bool success -string message -geometry_msgs/PoseStamped pose ---- -# Feedback: What we receive continuously while running -float32 time_elapsed -string status \ No newline at end of file diff --git a/src/perception/launch/action_launch.py b/src/perception/launch/action_launch.py deleted file mode 100644 index feab7f1..0000000 --- a/src/perception/launch/action_launch.py +++ /dev/null @@ -1,51 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - """Generates a launch description that can switch between robot and realsense inputs.""" - - # 1. Declare a launch argument 'input_source' - # This creates a variable that you can set from the command line. - # We'll set 'realsense' as the default value. - input_source_arg = DeclareLaunchArgument( - 'input_source', - default_value='realsense', - description='Input source for perception nodes. Can be "realsense" or "robot".' - ) - - # 2. YOLO Node - yolo_node = Node( - package='perception', - executable='action_yolo_node', - name='action_yolo_node', - output='screen', - parameters=[{ - # Use the value of the 'input_source' launch argument here - 'input_mode': LaunchConfiguration('input_source'), - 'model_type': 'fine_tuned', - 'conf_threshold': 0.6, - 'device': 'cuda' - }] - ) - - # 3. Pose Node - pose_node = Node( - package='perception', - executable='action_pose_node', - name='action_pose_node', - output='screen', - parameters=[{ - # Use the same 'input_source' launch argument here - 'input_mode': LaunchConfiguration('input_source') - }] - ) - - - # Return the LaunchDescription, including the argument and all nodes - return LaunchDescription([ - input_source_arg, - yolo_node, - pose_node, - ]) diff --git a/src/perception/perception/__init__.py b/src/perception/perception/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/perception/perception/action_pose_pca.py b/src/perception/perception/action_pose_pca.py deleted file mode 100644 index fef1b2a..0000000 --- a/src/perception/perception/action_pose_pca.py +++ /dev/null @@ -1,232 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import PointCloud2, PointField, Image -from vision_msgs.msg import Detection2DArray -from geometry_msgs.msg import PoseStamped, TransformStamped -from std_msgs.msg import Header - -import message_filters -from cv_bridge import CvBridge -import numpy as np -import struct -import sensor_msgs_py.point_cloud2 as pc2 -from transforms3d.quaternions import mat2quat -from tf2_ros import TransformBroadcaster -from std_srvs.srv import SetBool # <-- NEW: Service Import - - -class PointCloudCropperNode(Node): - def __init__(self): - super().__init__('pointcloud_cropper_node') - - self.bridge = CvBridge() - - # --- PARAMETERS --- - self.declare_parameter('input_mode', 'realsense') - input_mode = self.get_parameter('input_mode').get_parameter_value().string_value - - # Determine topics based on mode - if input_mode == 'robot': - self.pc_topic = '/camera/depth/color/points' - self.img_topic = '/camera/color/image_raw' - elif input_mode == 'realsense': - self.pc_topic = '/camera/camera/depth/color/points' - self.img_topic = '/camera/camera/color/image_raw' - else: - self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") - self.pc_topic = '/camera/camera/depth/color/points' - self.img_topic = '/camera/camera/color/image_raw' - - self.get_logger().info(f"Using input mode: '{input_mode}' with topics: {self.pc_topic}, {self.img_topic}") - - # --- 1. SETUP PUBLISHERS (Always Active) --- - self.pc_pub = self.create_publisher(PointCloud2, '/cropped_pointcloud', 10) - self.pose_pub = self.create_publisher(PoseStamped, '/object_pose', 10) - self.tf_broadcaster = TransformBroadcaster(self) - - # --- 2. OPTIMIZATION: INITIALIZE STATE --- - # We do NOT create subscribers or synchronizers here. - self.pc_sub = None - self.img_sub = None - self.det_sub = None - self.ts = None - self.is_active = False - - # --- 3. CREATE TOGGLE SERVICE --- - self.srv = self.create_service(SetBool, 'toggle_pose', self.toggle_callback) - - self.get_logger().info('PointCloud Cropper Node Ready in STANDBY mode.') - self.get_logger().info('Send "True" to /toggle_pose to start synchronization.') - - def toggle_callback(self, request, response): - """Service to Turn Processing ON or OFF""" - if request.data: # ENABLE - if not self.is_active: - self.get_logger().info("ACTIVATING: Starting Time Synchronizer...") - - # Create the message filters - self.pc_sub = message_filters.Subscriber(self, PointCloud2, self.pc_topic) - self.img_sub = message_filters.Subscriber(self, Image, self.img_topic) - self.det_sub = message_filters.Subscriber(self, Detection2DArray, '/detections') - - # Create the Synchronizer - self.ts = message_filters.ApproximateTimeSynchronizer( - [self.pc_sub, self.det_sub, self.img_sub], - queue_size=10, - slop=0.1 - ) - self.ts.registerCallback(self.sync_callback) - - self.is_active = True - response.message = "Pose Node Activated" - response.success = True - else: - response.message = "Already Active" - response.success = True - - else: # DISABLE - if self.is_active: - self.get_logger().info("DEACTIVATING: Destroying Synchronizer to save CPU...") - - # message_filters.Subscriber wrappers don't have a clean 'destroy' method - # exposed easily, but destroying the underlying subscription works. - # However, dropping the reference to the synchronizer stops the callbacks. - - # Best practice: explicitly destroy the underlying rclpy subscriptions - if self.pc_sub: self.destroy_subscription(self.pc_sub.sub) - if self.img_sub: self.destroy_subscription(self.img_sub.sub) - if self.det_sub: self.destroy_subscription(self.det_sub.sub) - - self.pc_sub = None - self.img_sub = None - self.det_sub = None - self.ts = None - - self.is_active = False - response.message = "Pose Node Deactivated" - response.success = True - else: - response.message = "Already Inactive" - response.success = True - - return response - - def sync_callback(self, cloud_msg, detection_msg, image_msg): - # This callback logic remains EXACTLY the same as your original code - try: - color_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding='bgr8') - except Exception as e: - self.get_logger().error(f"Image conversion error: {e}") - return - - pc_width = cloud_msg.width - pc_height = cloud_msg.height - - cloud_points = np.array([ - [x, y, z] - for x, y, z in pc2.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=False) - ]).reshape((pc_height, pc_width, 3)) - - all_colored_points = [] - - for idx, detection in enumerate(detection_msg.detections): - detected_class = detection.results[0].hypothesis.class_id - - cx = int(detection.bbox.center.position.x) - cy = int(detection.bbox.center.position.y) - w = int(detection.bbox.size_x) - h = int(detection.bbox.size_y) - - xmin = max(cx - w // 2, 0) - xmax = min(cx + w // 2, pc_width) - ymin = max(cy - h // 2, 0) - ymax = min(cy + h // 2, pc_height) - - cropped_points = cloud_points[ymin:ymax, xmin:xmax, :].reshape(-1, 3) - cropped_colors = color_image[ymin:ymax, xmin:xmax, :].reshape(-1, 3) - - valid_mask = ~np.isnan(cropped_points).any(axis=1) - cropped_points = cropped_points[valid_mask] - cropped_colors = cropped_colors[valid_mask] - - for pt, color in zip(cropped_points, cropped_colors): - x, y, z = pt - b, g, r = color - rgb = struct.unpack('f', struct.pack('I', (int(r) << 16) | (int(g) << 8) | int(b)))[0] - all_colored_points.append([x, y, z, rgb]) - - self.get_logger().info( - f"Cropped '{detected_class}' object {idx}: [{xmin}:{xmax}, {ymin}:{ymax}] -> {len(cropped_points)} valid points" - ) - - if len(cropped_points) >= 3: - centroid = np.mean(cropped_points, axis=0) - centered = cropped_points - centroid - _, _, vh = np.linalg.svd(centered, full_matrices=False) - R = vh.T - - T = np.eye(4) - T[:3, :3] = R - T[:3, 3] = centroid - - quat_wxyz = mat2quat(T[:3, :3]) - quat = [quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]] - - pose_msg = PoseStamped() - pose_msg.header.stamp = self.get_clock().now().to_msg() - pose_msg.header.frame_id = cloud_msg.header.frame_id - pose_msg.pose.position.x = float(centroid[0]) - pose_msg.pose.position.y = float(centroid[1]) - pose_msg.pose.position.z = float(centroid[2]) - pose_msg.pose.orientation.x = float(quat[0]) - pose_msg.pose.orientation.y = float(quat[1]) - pose_msg.pose.orientation.z = float(quat[2]) - pose_msg.pose.orientation.w = float(quat[3]) - - self.pose_pub.publish(pose_msg) - - t = TransformStamped() - t.header.stamp = self.get_clock().now().to_msg() - t.header.frame_id = cloud_msg.header.frame_id - t.child_frame_id = f'object_frame_{idx}' - t.transform.translation.x = float(centroid[0]) - t.transform.translation.y = float(centroid[1]) - t.transform.translation.z = float(centroid[2]) - t.transform.rotation.x = float(quat[0]) - t.transform.rotation.y = float(quat[1]) - t.transform.rotation.z = float(quat[2]) - t.transform.rotation.w = float(quat[3]) - - self.tf_broadcaster.sendTransform(t) - self.get_logger().info(f"Published pose and TF for '{detected_class}' object {idx}") - - if all_colored_points: - header = Header() - header.stamp = self.get_clock().now().to_msg() - header.frame_id = cloud_msg.header.frame_id - - fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), - PointField(name='rgb', offset=12, datatype=PointField.FLOAT32, count=1), - ] - - cropped_pc = pc2.create_cloud(header, fields, all_colored_points) - self.pc_pub.publish(cropped_pc) - - -def main(args=None): - rclpy.init(args=args) - node = PointCloudCropperNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/perception/perception/action_vision_manager.py b/src/perception/perception/action_vision_manager.py deleted file mode 100644 index fba49b3..0000000 --- a/src/perception/perception/action_vision_manager.py +++ /dev/null @@ -1,249 +0,0 @@ -import time -import rclpy -from rclpy.node import Node -from rclpy.action import ActionServer, CancelResponse, GoalResponse -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.executors import MultiThreadedExecutor -from std_srvs.srv import SetBool -from geometry_msgs.msg import PoseStamped -from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy -import statistics - -from my_robot_interfaces.action import RunVision - -class VisionManager(Node): - def __init__(self): - super().__init__('vision_manager') - - # We use a ReentrantCallbackGroup to allow the Action Loop and the - # Subscriber Callback to run in parallel on the MultiThreadedExecutor - self.group = ReentrantCallbackGroup() - - # --- 1. ACTION SERVER --- - self._action_server = ActionServer( - self, - RunVision, - 'run_vision_pipeline', - execute_callback=self.execute_callback, - goal_callback=self.goal_callback, - cancel_callback=self.cancel_callback, - callback_group=self.group - ) - - # --- 2. SERVICE CLIENTS (Talks to YOLO and Pose nodes) --- - self.yolo_client = self.create_client(SetBool, '/toggle_yolo', callback_group=self.group) - self.pose_client = self.create_client(SetBool, '/toggle_pose', callback_group=self.group) - - # --- 3. STATE MANAGEMENT --- - self.captured_poses = [] - self.collection_active = False # The "Gate" flag - - # --- 4. SUBSCRIBER --- - # Reliable QoS ensures we don't miss packets if they are sent correctly - qos_profile = QoSProfile( - reliability=ReliabilityPolicy.RELIABLE, - history=HistoryPolicy.KEEP_LAST, - depth=10 - ) - - self.pose_sub = self.create_subscription( - PoseStamped, - '/object_pose', - self.pose_callback, - qos_profile, - callback_group=self.group - ) - - self.vis_pub = self.create_publisher( - PoseStamped, - '/visualized_average_pose', - 10 - ) - - self.get_logger().info("Vision Manager Ready. Initializing sensors to OFF state...") - - # --- 5. INITIAL CLEANUP TIMER --- - # We run this ONCE to ensure sensors start in a clean "Standby" state. - self.init_timer = self.create_timer(1.0, self.initial_cleanup) - - async def initial_cleanup(self): - """Forces connected nodes to sleep on startup so we start fresh.""" - - # CRITICAL FIX: Cancel the timer so this function NEVER runs again - if self.init_timer: - self.init_timer.cancel() - self.init_timer = None - - self.get_logger().info("System Startup: Ensuring sensors are in STANDBY mode...") - await self.set_nodes_state(False) - - def pose_callback(self, msg): - """ - Only process data if the Action is running (collection_active is True). - Otherwise, ignore the noise to prevent log spam and dirty data. - """ - if not self.collection_active: - return # IGNORE BACKGROUND NOISE - - self.get_logger().info(f"Received Pose: x={msg.pose.position.x:.2f}") - self.captured_poses.append(msg) - - def goal_callback(self, goal_request): - return GoalResponse.ACCEPT - - def cancel_callback(self, goal_handle): - return CancelResponse.ACCEPT - - async def execute_callback(self, goal_handle): - self.get_logger().info('Goal Received. Opening the Gate...') - - # 1. Reset Buffer for new run - self.captured_poses = [] - result = RunVision.Result() - feedback = RunVision.Feedback() - duration = goal_handle.request.duration_seconds - - # 2. Wake up nodes - if not await self.set_nodes_state(True): - goal_handle.abort() - result.success = False - result.message = "Failed to wake sensors" - return result - - # 3. Enable Collection (Open Gate) - self.collection_active = True - - # 4. Run Timer Loop - start_time = time.time() - feedback.status = "Collecting Poses" - - while (time.time() - start_time) < duration: - # Check Cancel - if goal_handle.is_cancel_requested: - self.collection_active = False # Close gate - await self.set_nodes_state(False) - goal_handle.canceled() - result.success = False - result.message = "Canceled" - return result - - # Publish Feedback - feedback.time_elapsed = time.time() - start_time - goal_handle.publish_feedback(feedback) - time.sleep(0.1) - - # 5. Disable Collection (Close Gate) - self.collection_active = False - - # 6. Shut Down Sensors - await self.set_nodes_state(False) - - # 7. Process Data - if len(self.captured_poses) > 0: - final_pose = self.calculate_average_pose(self.captured_poses) - - # Publish to RViz so we can see the result - self.vis_pub.publish(final_pose) - self.get_logger().info("Published final median pose to /visualized_average_pose") - - result.success = True - result.message = f"Success. Calculated Median of {len(self.captured_poses)} frames." - result.pose = final_pose - goal_handle.succeed() - else: - result.success = False - result.message = "Time finished, but no objects were detected." - self.get_logger().warn("Finished with 0 poses.") - goal_handle.abort() - - return result - - def calculate_average_pose(self, poses): - """ - Calculates the 'Truncated Mean' (Interquartile Mean). - 1. Sorts data. - 2. Removes top 25% and bottom 25% (outliers). - 3. Averages the remaining middle 50%. - """ - if not poses: return PoseStamped() - - # Helper function to get truncated mean of a list of numbers - def get_truncated_mean(data): - if not data: return 0.0 - n = len(data) - if n < 3: return statistics.mean(data) # Too few to truncate - - sorted_data = sorted(data) - # Remove top and bottom 25% - cut_amount = int(n * 0.25) - # Slice the middle - middle_data = sorted_data[cut_amount : n - cut_amount] - - return statistics.mean(middle_data) - - # 1. Calculate Truncated Mean for Position - final_x = get_truncated_mean([p.pose.position.x for p in poses]) - final_y = get_truncated_mean([p.pose.position.y for p in poses]) - final_z = get_truncated_mean([p.pose.position.z for p in poses]) - - # 2. Find the orientation from the pose closest to this new calculated position - # (We still shouldn't average quaternions simply, so we pick the best representative) - best_idx = 0 - min_dist = float('inf') - - for i, p in enumerate(poses): - dist = (p.pose.position.x - final_x)**2 + \ - (p.pose.position.y - final_y)**2 + \ - (p.pose.position.z - final_z)**2 - if dist < min_dist: - min_dist = dist - best_idx = i - - # 3. Construct Final Pose - final_pose = PoseStamped() - final_pose.header = poses[best_idx].header - final_pose.header.stamp = self.get_clock().now().to_msg() - - final_pose.pose.position.x = final_x - final_pose.pose.position.y = final_y - final_pose.pose.position.z = final_z - final_pose.pose.orientation = poses[best_idx].pose.orientation - - return final_pose - - async def set_nodes_state(self, active: bool): - """Helper to call services asynchronously.""" - req = SetBool.Request() - req.data = active - - # Small timeout check to avoid blocking if nodes aren't up - if not self.yolo_client.wait_for_service(timeout_sec=1.0): - self.get_logger().warn("YOLO Service not available") - return False - if not self.pose_client.wait_for_service(timeout_sec=1.0): - self.get_logger().warn("Pose Service not available") - return False - - future_yolo = self.yolo_client.call_async(req) - future_pose = self.pose_client.call_async(req) - - try: - await future_yolo - await future_pose - return True - except Exception as e: - self.get_logger().error(f"Service toggle failed: {e}") - return False - -def main(args=None): - rclpy.init(args=args) - node = VisionManager() - - # CRITICAL: MultiThreadedExecutor is required for Callbacks to run - # while the Action Loop is active. - executor = MultiThreadedExecutor() - rclpy.spin(node, executor=executor) - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/perception/perception/action_yolo_object_detection.py b/src/perception/perception/action_yolo_object_detection.py deleted file mode 100644 index 303069f..0000000 --- a/src/perception/perception/action_yolo_object_detection.py +++ /dev/null @@ -1,197 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Image -from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose -from cv_bridge import CvBridge -from ultralytics import YOLO -from ultralytics.engine.results import Boxes -import torch -import cv2 -import os -from ament_index_python.packages import get_package_share_directory -from pathlib import Path -from std_srvs.srv import SetBool # Required for the toggle service - -def get_package_name_from_path(file_path): - """Dynamically find the package name from the file path.""" - p = Path(file_path) - try: - package_parts = p.parts[p.parts.index('site-packages') + 1:] - return package_parts[0] - except ValueError: - # Fallback if not installed in site-packages (e.g. running from source) - return 'perception' - -class YoloDetectorNode(Node): - def __init__(self): - super().__init__('yolo_detector_node') - self.package_name = get_package_name_from_path(__file__) - self.bridge = CvBridge() - - # --- PARAMETERS --- - self.declare_parameter('model_type', 'default') - self.declare_parameter('input_mode', 'realsense') - self.declare_parameter('model_path', '') - self.declare_parameter('conf_threshold', 0.6) - self.declare_parameter('device', '') - self.declare_parameter('class_names', []) - - model_type = self.get_parameter('model_type').get_parameter_value().string_value - input_mode = self.get_parameter('input_mode').get_parameter_value().string_value - explicit_model_path = self.get_parameter('model_path').get_parameter_value().string_value - self.conf_threshold = self.get_parameter('conf_threshold').get_parameter_value().double_value - device_param = self.get_parameter('device').get_parameter_value().string_value - class_names_param = self.get_parameter('class_names').get_parameter_value().string_array_value - - # --- MODEL PATH LOGIC --- - if explicit_model_path: - model_path = explicit_model_path - else: - try: - package_share_directory = get_package_share_directory(self.package_name) - if model_type == 'fine_tuned': - model_path = os.path.join(package_share_directory, 'models', 'fine_tuned.pt') - else: - model_path = os.path.join(package_share_directory, 'models', 'yolov8n.pt') - except Exception: - # Fallback for when running locally/testing without full install - model_path = 'yolov8n.pt' - self.get_logger().warn(f"Could not find package share directory. Defaulting model path to: {model_path}") - - self.get_logger().info(f"Using model type '{model_type}' from: {model_path}") - - # --- 1. LOAD MODEL (Heavy operation, done ONCE at startup) --- - self.get_logger().info("Loading YOLO model... (This stays in RAM)") - try: - if device_param: - self.model = YOLO(model_path) - try: - self.model.to(device_param) - self.get_logger().info(f"Model moved to device: {device_param}") - except Exception as e: - self.get_logger().warn(f"Failed to move model to device '{device_param}': {e}") - else: - self.model = YOLO(model_path) - except Exception as e: - self.get_logger().error(f"Failed to load YOLO model from '{model_path}': {e}") - raise - - # Optional override for class names - self.class_names = None - if class_names_param: - self.class_names = list(class_names_param) - - # --- 2. DETERMINE TOPIC (Save for later) --- - if input_mode == 'robot': - self.image_topic = '/camera/color/image_raw' - elif input_mode == 'realsense': - self.image_topic = '/camera/camera/color/image_raw' - else: - self.get_logger().warn(f"Unknown input_mode '{input_mode}', defaulting to 'realsense'") - self.image_topic = '/camera/camera/color/image_raw' - - # --- 3. SETUP PUBLISHERS --- - self.annotated_image_pub = self.create_publisher(Image, '/annotated_image', 10) - self.detection_pub = self.create_publisher(Detection2DArray, '/detections', 10) - - # --- 4. OPTIMIZATION / SOFT LIFECYCLE --- - # We do NOT subscribe here. We start in "Standby" mode. - self.image_sub = None - - # Create the service to wake up the node - self.srv = self.create_service(SetBool, 'toggle_yolo', self.toggle_callback) - - self.get_logger().info(f"YOLOv8 Node Initialized in STANDBY mode.") - self.get_logger().info(f"Send 'True' to service '/toggle_yolo' to start processing {self.image_topic}") - - def toggle_callback(self, request, response): - """Service callback to Turn processing ON or OFF""" - if request.data: # REQUEST: ENABLE - if self.image_sub is None: - self.get_logger().info(f"ACTIVATING: Subscribing to {self.image_topic}...") - self.image_sub = self.create_subscription( - Image, self.image_topic, self.image_callback, 10 - ) - response.message = "YOLO Activated" - response.success = True - else: - response.message = "Already Active" - response.success = True - else: # REQUEST: DISABLE - if self.image_sub is not None: - self.get_logger().info("DEACTIVATING: Unsubscribing to save CPU...") - self.destroy_subscription(self.image_sub) - self.image_sub = None - response.message = "YOLO Deactivated" - response.success = True - else: - response.message = "Already Inactive" - response.success = True - return response - - def image_callback(self, msg): - """This function ONLY runs when the node is Activated""" - try: - cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') - except Exception as e: - self.get_logger().error(f'Image conversion error: {e}') - return - - # Inference - results = self.model(cv_image, verbose=False) # verbose=False keeps terminal clean - detection_array_msg = Detection2DArray() - detection_array_msg.header = msg.header - - for result in results: - filtered_boxes = [box for box in result.boxes if float(box.conf) >= self.conf_threshold] - - # 1. Publish Annotated Image - annotated_image = result.plot() - try: - annotated_msg = self.bridge.cv2_to_imgmsg(annotated_image, encoding='bgr8') - annotated_msg.header = msg.header - self.annotated_image_pub.publish(annotated_msg) - except Exception as e: - self.get_logger().error(f'Annotated image conversion error: {e}') - - # 2. Publish Detections - for box in filtered_boxes: - detection_msg = Detection2D() - detection_msg.header = msg.header - - hypothesis = ObjectHypothesisWithPose() - try: - if self.class_names and int(box.cls) < len(self.class_names): - class_name = self.class_names[int(box.cls)] - else: - class_name = self.model.names[int(box.cls)] - except Exception: - class_name = str(int(box.cls)) - - hypothesis.hypothesis.class_id = class_name - hypothesis.hypothesis.score = float(box.conf) - detection_msg.results.append(hypothesis) - - xywh = box.xywh.cpu().numpy().flatten() - detection_msg.bbox.center.position.x = float(xywh[0]) - detection_msg.bbox.center.position.y = float(xywh[1]) - detection_msg.bbox.size_x = float(xywh[2]) - detection_msg.bbox.size_y = float(xywh[3]) - - detection_array_msg.detections.append(detection_msg) - - self.detection_pub.publish(detection_array_msg) - -def main(args=None): - rclpy.init(args=args) - node = YoloDetectorNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/perception/perception/rviz/pose_estimation.rviz b/src/perception/perception/rviz/pose_estimation.rviz deleted file mode 100644 index e7bf5cf..0000000 --- a/src/perception/perception/rviz/pose_estimation.rviz +++ /dev/null @@ -1,219 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PointCloud21 - - /TF1 - Splitter Ratio: 0.5 - Tree Height: 348 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /annotated_image - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /cropped_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: true - camera_color_frame: - Value: true - camera_color_optical_frame: - Value: true - camera_depth_frame: - Value: true - camera_depth_optical_frame: - Value: true - camera_link: - Value: true - object_frame_0: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - camera_link: - camera_color_frame: - camera_color_optical_frame: - {} - camera_depth_frame: - camera_depth_optical_frame: - object_frame_0: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: camera_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 2.855905532836914 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 1.2362849712371826 - Y: -0.21183088421821594 - Z: -0.2512143552303314 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: -0.05460222065448761 - Target Frame: - Value: Orbit (rviz) - Yaw: 3.0917561054229736 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 846 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002acfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000001ea000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000022f000000bc0000000000000000fb0000000a0049006d006100670065010000022f000000bc0000001700ffffff000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1200 - X: 66 - Y: 60 diff --git a/src/perception/resource/perception b/src/perception/resource/perception deleted file mode 100644 index e69de29..0000000 diff --git a/src/perception/setup.cfg b/src/perception/setup.cfg deleted file mode 100644 index be4fcdb..0000000 --- a/src/perception/setup.cfg +++ /dev/null @@ -1,6 +0,0 @@ -[develop] -script_dir=$base/lib/perception -[install] -install_scripts=$base/lib/perception -[build_scripts] -executable = /usr/bin/env python3 diff --git a/src/table_height_predictor/package.xml b/src/table_height_predictor/package.xml deleted file mode 100644 index 08125a4..0000000 --- a/src/table_height_predictor/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - table_height_predictor - 0.0.0 - TODO: Package description - mohsin - TODO: License declaration - - sensor_msgs - std_msgs - rclpy - image_transport - cv_bridge - python3-opencv - opencv4 - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - \ No newline at end of file diff --git a/src/table_height_predictor/resource/table_height_predictor b/src/table_height_predictor/resource/table_height_predictor deleted file mode 100644 index e69de29..0000000 diff --git a/src/table_height_predictor/rviz/table_height.rviz b/src/table_height_predictor/rviz/table_height.rviz deleted file mode 100644 index 203a42a..0000000 --- a/src/table_height_predictor/rviz/table_height.rviz +++ /dev/null @@ -1,258 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 138 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Image1 - - /PointCloud21 - - /TF1 - - /PointCloud22 - Splitter Ratio: 0.5 - Tree Height: 1531 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /table_segmentation_image - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera/depth/color/points - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: true - camera_color_frame: - Value: true - camera_depth_frame: - Value: true - camera_link: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - camera_link: - camera_color_frame: - {} - camera_depth_frame: - {} - Update Interval: 0 - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Marker - Namespaces: - table_center: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /table_marker - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 3.0160000324249268 - Min Value: 1.0190000534057617 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /table_points_debug - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: camera_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 1.535581350326538 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.1352316290140152 - Y: 0.32156407833099365 - Z: 0.4516058564186096 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: true - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.4647964239120483 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.534939289093018 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 2742 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000003c0000009acfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000006f70000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000022f000000bc0000000000000000fb0000000a0049006d0061006700650100000773000002a90000002800ffffff000000010000015d000009acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000009ac0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000137c0000005efc0100000002fb0000000800540069006d006501000000000000137c0000047a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000e47000009ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 4988 - X: 132 - Y: 64 diff --git a/src/table_height_predictor/setup.cfg b/src/table_height_predictor/setup.cfg deleted file mode 100644 index 1d892df..0000000 --- a/src/table_height_predictor/setup.cfg +++ /dev/null @@ -1,6 +0,0 @@ -[develop] -script_dir=$base/lib/table_height_predictor -[install] -install_scripts=$base/lib/table_height_predictor -[build_scripts] -executable = /usr/bin/env python3 diff --git a/src/table_height_predictor/setup.py b/src/table_height_predictor/setup.py deleted file mode 100644 index 60af6e8..0000000 --- a/src/table_height_predictor/setup.py +++ /dev/null @@ -1,27 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'table_height_predictor' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='mohsin', - maintainer_email='mohsinalimirxa@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'detect_floor = table_height_predictor.floor_detector_node:main', - 'table_height = table_height_predictor.table_height_node:main', - ], - }, -) diff --git a/src/table_height_predictor/table_height_predictor/__init__.py b/src/table_height_predictor/table_height_predictor/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/table_height_predictor/table_height_predictor/table_height_node.py b/src/table_height_predictor/table_height_predictor/table_height_node.py deleted file mode 100644 index 758f9df..0000000 --- a/src/table_height_predictor/table_height_predictor/table_height_node.py +++ /dev/null @@ -1,242 +0,0 @@ -import rclpy -from rclpy.node import Node -import message_filters -from sensor_msgs.msg import Image, PointCloud2 -from visualization_msgs.msg import Marker, MarkerArray # Changed to MarkerArray -from cv_bridge import CvBridge -import sensor_msgs_py.point_cloud2 as pc2 -import numpy as np -import cv2 -import torch -from ultralytics import YOLOWorld, SAM -from sklearn.linear_model import RANSACRegressor # Added for Floor Math - -class TableHeightNode(Node): - def __init__(self): - super().__init__('table_height_estimator') - - # --- CONFIGURATION --- - self.img_topic = '/camera/color/image_raw' - self.pc_topic = '/camera/depth/color/points' - self.custom_classes = ["white standing desk", "white table surface"] - self.conf_threshold = 0.2 - self.device = 'cuda' if torch.cuda.is_available() else 'cpu' - - # --- PUBLISHERS --- - # We use MarkerArray now to draw Table, Floor, Line, and Text all at once - self.marker_pub = self.create_publisher(MarkerArray, '/table_height_visualization', 10) - self.debug_pc_pub = self.create_publisher(PointCloud2, '/table_points_debug', 10) - self.seg_img_pub = self.create_publisher(Image, '/table_segmentation_image', 10) - - # --- MODEL INITIALIZATION --- - self.get_logger().info(f"Loading models on {self.device}...") - self.det_model = YOLOWorld('yolov8l-worldv2.pt') - self.det_model.set_classes(self.custom_classes) - self.det_model.to(self.device) - self.seg_model = SAM('sam_b.pt') - self.seg_model.to(self.device) - - # --- ROS SETUP --- - self.bridge = CvBridge() - self.img_sub = message_filters.Subscriber(self, Image, self.img_topic) - self.pc_sub = message_filters.Subscriber(self, PointCloud2, self.pc_topic) - self.ts = message_filters.ApproximateTimeSynchronizer( - [self.img_sub, self.pc_sub], queue_size=10, slop=0.1 - ) - self.ts.registerCallback(self.callback) - - self.get_logger().info("Table Height Node Initialized.") - - def callback(self, img_msg, pc_msg): - try: - cv_image = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') - except Exception as e: - self.get_logger().error(f"CvBridge Error: {e}") - return - - # 1. YOLO-World (Find Table) - det_results = self.det_model.predict(cv_image, conf=self.conf_threshold, verbose=False) - bboxes = det_results[0].boxes.xyxy.tolist() - if not bboxes: return - - # 2. SAM (Segment Table) - table_box = [bboxes[0]] - seg_results = self.seg_model(cv_image, bboxes=table_box, verbose=False) - if seg_results[0].masks is None: return - table_mask = seg_results[0].masks.data[0].cpu().numpy() - - # 3. Visualization Image - self.publish_debug_image(cv_image, table_mask, bboxes[0], img_msg.header) - - # 4. Calculate Height Logic - self.process_point_cloud(pc_msg, table_mask) - - def process_point_cloud(self, pc_msg, table_mask): - if pc_msg.height <= 1: return - - # --- A. Parse PointCloud into (H, W, 3) Array --- - # (Optimized parsing logic) - raw_data = np.frombuffer(pc_msg.data, dtype=np.uint8) - try: - raw_data = raw_data.reshape(pc_msg.height, pc_msg.row_step) - except ValueError: return - - bytes_per_pixel = pc_msg.point_step - raw_data = raw_data[:, :pc_msg.width * bytes_per_pixel] - pixel_chunks = raw_data.reshape(pc_msg.height, pc_msg.width, bytes_per_pixel) - - # Get field offsets - off_x, off_y, off_z = 0, 4, 8 - for field in pc_msg.fields: - if field.name == 'x': off_x = field.offset - if field.name == 'y': off_y = field.offset - if field.name == 'z': off_z = field.offset - - x = pixel_chunks[:, :, off_x : off_x+4].view(dtype=np.float32).squeeze() - y = pixel_chunks[:, :, off_y : off_y+4].view(dtype=np.float32).squeeze() - z = pixel_chunks[:, :, off_z : off_z+4].view(dtype=np.float32).squeeze() - - points_3d = np.dstack((x, y, z)) # Shape: (H, W, 3) - - # --- B. Get Table Center --- - table_pts = points_3d[table_mask] - valid_table = table_pts[~np.isnan(table_pts).any(axis=1)] - if valid_table.shape[0] < 50: return - - t_x = np.median(valid_table[:, 0]) - t_y = np.median(valid_table[:, 1]) - t_z = np.median(valid_table[:, 2]) - - # --- C. Get Floor Position (RANSAC) --- - # Strategy: Use points from bottom 50% of image that are NOT the table - h, w, _ = points_3d.shape - # Create a mask for "potential floor" (bottom half of image) - floor_region_mask = np.zeros((h, w), dtype=bool) - floor_region_mask[int(h*0.5):, :] = True - - # Exclude the table itself from floor candidates - floor_candidates_mask = floor_region_mask & (~table_mask) - - floor_pts = points_3d[floor_candidates_mask] - # Subsample for speed (take every 10th point) - floor_pts = floor_pts[::10] - valid_floor = floor_pts[~np.isnan(floor_pts).any(axis=1)] - - floor_y_at_table = None - - if valid_floor.shape[0] > 100: - # RANSAC: Fit plane y = f(x, z) - # We assume floor height (y) depends on horizontal (x) and depth (z) - X_in = valid_floor[:, [0, 2]] # Inputs: x, z - Y_out = valid_floor[:, 1] # Output: y (height) - - ransac = RANSACRegressor(residual_threshold=0.05) - try: - ransac.fit(X_in, Y_out) - - # KEY STEP: Predict floor Y specifically at Table's X and Z - floor_y_at_table = ransac.predict([[t_x, t_z]])[0] - - # Sanity Check: Floor must be visibly lower (higher Y value) than table - if floor_y_at_table < t_y + 0.1: - floor_y_at_table = None # Invalid, floor appeared above table - except: - pass - - # --- D. Visualize --- - self.publish_markers(t_x, t_y, t_z, floor_y_at_table, pc_msg.header) - - # Publish Debug Cloud (Table Only) - debug_cloud = pc2.create_cloud_xyz32(pc_msg.header, valid_table) - self.debug_pc_pub.publish(debug_cloud) - - def publish_markers(self, tx, ty, tz, fy, header): - marker_array = MarkerArray() - - # 1. Table Marker (Red Sphere) - m_table = Marker() - m_table.header = header - m_table.ns = "table" - m_table.id = 0 - m_table.type = Marker.SPHERE - m_table.action = Marker.ADD - m_table.pose.position.x, m_table.pose.position.y, m_table.pose.position.z = float(tx), float(ty), float(tz) - m_table.scale.x = m_table.scale.y = m_table.scale.z = 0.08 - m_table.color.r, m_table.color.g, m_table.color.b, m_table.color.a = 1.0, 0.0, 0.0, 1.0 - marker_array.markers.append(m_table) - - log_msg = f"Table Z: {tz:.2f}m" - - if fy is not None: - # Calculate Height - height_meters = abs(fy - ty) - log_msg += f" | Floor Est: {fy:.2f}m | HEIGHT: {height_meters:.3f}m" - - # 2. Floor Marker (Green Flat Cube) - m_floor = Marker() - m_floor.header = header - m_floor.ns = "floor" - m_floor.id = 1 - m_floor.type = Marker.CUBE - m_floor.action = Marker.ADD - # Uses Table X/Z but Floor Y - m_floor.pose.position.x, m_floor.pose.position.y, m_floor.pose.position.z = float(tx), float(fy), float(tz) - m_floor.scale.x, m_floor.scale.z = 0.2, 0.2 - m_floor.scale.y = 0.005 # Thin - m_floor.color.r, m_floor.color.g, m_floor.color.b, m_floor.color.a = 0.0, 1.0, 0.0, 1.0 - marker_array.markers.append(m_floor) - - # 3. Connection Line (Yellow) - m_line = Marker() - m_line.header = header - m_line.ns = "line" - m_line.id = 2 - m_line.type = Marker.LINE_LIST - m_line.action = Marker.ADD - m_line.scale.x = 0.005 # Line thickness - m_line.color.r, m_line.color.g, m_line.color.b, m_line.color.a = 1.0, 1.0, 0.0, 1.0 - m_line.points.append(m_table.pose.position) - m_line.points.append(m_floor.pose.position) - marker_array.markers.append(m_line) - - # 4. Text Label (Height Value) - m_text = Marker() - m_text.header = header - m_text.ns = "text" - m_text.id = 3 - m_text.type = Marker.TEXT_VIEW_FACING - m_text.action = Marker.ADD - m_text.text = f"{height_meters:.2f}m" - m_text.pose.position.x = float(tx) + 0.15 # Offset text to right - m_text.pose.position.y = (ty + fy) / 2.0 # Center of line - m_text.pose.position.z = float(tz) - m_text.scale.z = 0.05 # Text size - m_text.color.r, m_text.color.g, m_text.color.b, m_text.color.a = 1.0, 1.0, 1.0, 1.0 - marker_array.markers.append(m_text) - - self.get_logger().info(log_msg) - self.marker_pub.publish(marker_array) - - def publish_debug_image(self, cv_image, mask, bbox, header): - overlay = cv_image.copy() - overlay[mask] = [0, 255, 0] - blended = cv2.addWeighted(overlay, 0.4, cv_image, 0.6, 0) - x1, y1, x2, y2 = map(int, bbox) - cv2.rectangle(blended, (x1, y1), (x2, y2), (0,0,255), 2) - - msg = self.bridge.cv2_to_imgmsg(blended, encoding="bgr8") - msg.header = header - self.seg_img_pub.publish(msg) - -def main(args=None): - rclpy.init(args=args) - node = TableHeightNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: pass - finally: - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/table_height_predictor/test/test_copyright.py b/src/table_height_predictor/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/src/table_height_predictor/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/table_height_predictor/test/test_flake8.py b/src/table_height_predictor/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/table_height_predictor/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/table_height_predictor/test/test_pep257.py b/src/table_height_predictor/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/table_height_predictor/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings'