diff --git a/src/igvc_cv/igvc_cv/__init__.py b/src/igvc_cv/igvc_cv/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py new file mode 100644 index 00000000..b1e61f64 --- /dev/null +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -0,0 +1,139 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from message_filters import ApproximateTimeSynchronizer, Subscriber +import cv2 +import numpy as np + +from sensor_msgs.msg import PointCloud2 +from std_msgs.msg import Header +import sensor_msgs_py.point_cloud2 as pc2 + +class CVNode(Node): + def __init__(self): + super().__init__('cv_node') + self.bridge = CvBridge() + + # Gaussian blur + self.declare_parameter("blur_kernel_size", 11) + self.declare_parameter("blur_sigma_x", 33.0) + self.declare_parameter("blur_sigma_y", 33.0) + + # Canny edge detection + self.declare_parameter("canny_low", 10) + self.declare_parameter("canny_high", 100) + + # Hough Line Transform + self.declare_parameter("hough_rho", 2.0) + self.declare_parameter("hough_theta", np.pi / 180) + self.declare_parameter("hough_threshold", 10) + self.declare_parameter("hough_min_line_length", 4) + self.declare_parameter("hough_max_line_gap", 5) + + self.declare_parameter("line_thickness", 10) + + # Synchronised RGB + PointCloud subscribers + self.rgb_sub = Subscriber(self, Image, '/camera/camera/color/image_raw') + self.pc_sub = Subscriber(self, Image, '/camera/camera/depth/image_rect_raw') + self.sync = ApproximateTimeSynchronizer( + [self.rgb_sub, self.pc_sub], + queue_size=10, + slop=0.05 + ) + self.sync.registerCallback(self.process) + # ===== Publishers ===== + self.image_pub = self.create_publisher(Image, 'image_processed', 10) + + # Camera parameters based on factor calibration file + self.fx = 1401.07 + self.fy = 1401.07 + self.cx = 1062.32 + self.cy = 634.124 + + def process(self, rgb_msg: Image, depth_msg: Image): + self.get_logger().info("Called Process()") + # Fetch parameters + blur_k = self.get_parameter("blur_kernel_size").value + blur_sx = self.get_parameter("blur_sigma_x").value + blur_sy = self.get_parameter("blur_sigma_y").value + canny_low = self.get_parameter("canny_low").value + canny_high = self.get_parameter("canny_high").value + hough_rho = self.get_parameter("hough_rho").value + hough_theta = self.get_parameter("hough_theta").value + hough_thresh = self.get_parameter("hough_threshold").value + hough_min_len = self.get_parameter("hough_min_line_length").value + hough_max_gap = self.get_parameter("hough_max_line_gap").value + thickness = self.get_parameter("line_thickness").value + + # Convert ROS image to OpenCV + frame = self.bridge.imgmsg_to_cv2(rgb_msg, 'bgr8') + depth_frame = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding='passthrough') + h, w = frame.shape[:2] + + # Convert to grayscale + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # Gaussian Blur + blur = cv2.GaussianBlur(gray, (blur_k, blur_k), blur_sx, blur_sy) + + # Canny edge detection + edges = cv2.Canny(blur, canny_low, canny_high) + + # Hough Line Transform + lines = cv2.HoughLinesP( + edges, + hough_rho, hough_theta, hough_thresh, + minLineLength=hough_min_len, + maxLineGap=hough_max_gap + ) + + # Mask of detected line segments + line_mask = np.zeros((h, w), dtype=np.uint8) + line_image = np.zeros_like(frame) + + if lines is not None: + for line in lines: + x1, y1, x2, y2 = line.reshape(4) + cv2.line(line_image, (x1, y1), (x2, y2), (0, 255, 255), thickness) + cv2.line(line_mask, (x1, y1), (x2, y2), 255, thickness) + + # Look up depth for each white pixel directly + white_v, white_u = np.where(line_mask > 0) + distances = depth_frame[white_v, white_u] + + valid = distances > 0 + white_u = white_u[valid] + white_v = white_v[valid] + distances = distances[valid] + + #points to 3D + Z = distances + X = (white_u - self.cx) * Z / self.fx + Y = (white_v - self.cy) * Z / self.fy + + #create pointcloud + points = np.vstack((X, Y, Z)).T + + header = Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = "" + + cloud_msg = pc2.create_cloud_xyz32(header, points.tolist()) + self.pub.publish(cloud_msg) + + """ + # Publish image + result = cv2.addWeighted(frame, 0.8, line_image, 1.0, 1) + ros_image = self.bridge.cv2_to_imgmsg(result, 'bgr8') + ros_image.header = rgb_msg.header + self.image_pub.publish(ros_image) + self.get_logger().info("Published Image") + """ + +def main(args=None): + rclpy.init(args=args) + node = CVNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() \ No newline at end of file diff --git a/src/igvc_cv/package.xml b/src/igvc_cv/package.xml new file mode 100644 index 00000000..0d121a7b --- /dev/null +++ b/src/igvc_cv/package.xml @@ -0,0 +1,23 @@ + + + + igvc_cv + 0.0.0 + Computer Vision for the 2026 IGVC Robot + Camden Laursen-Carr + MIT + + rclpy + sensor_msgs + cv_bridge + python3-opencv + python3-numpy + message_filters + + python3-pytest + + + ament_python + + + diff --git a/src/igvc_cv/resource/igvc_cv b/src/igvc_cv/resource/igvc_cv new file mode 100644 index 00000000..e69de29b diff --git a/src/igvc_cv/setup.cfg b/src/igvc_cv/setup.cfg new file mode 100644 index 00000000..b1195d67 --- /dev/null +++ b/src/igvc_cv/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/igvc_cv +[install] +install_scripts=$base/lib/igvc_cv diff --git a/src/igvc_cv/setup.py b/src/igvc_cv/setup.py new file mode 100644 index 00000000..71aa8243 --- /dev/null +++ b/src/igvc_cv/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'igvc_cv' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools', 'numpy', 'opencv-python', 'sensor-msgs-py'], + zip_safe=True, + maintainer='Camden Laursen-Carr', + maintainer_email='laursc@rpi.edu', + description='Computer Vision for the 2026 IGVC Robot', + license='MIT', + extras_require={ + }, + entry_points={ + 'console_scripts': [ + 'cv_node = igvc_cv.cv_node:main', + ], + }, +)