From a6ea1f5173ae863a6060abe6f75dfe5cfe12386d Mon Sep 17 00:00:00 2001 From: = Date: Sat, 21 Feb 2026 05:40:42 +0000 Subject: [PATCH 01/23] Added CV ROS2 Node (#39) --- src/igvc_cv/igvc_cv/__init__.py | 0 src/igvc_cv/igvc_cv/cv_node.py | 50 +++++++++++++++++++++++++++++++++ src/igvc_cv/package.xml | 20 +++++++++++++ src/igvc_cv/resource/igvc_cv | 0 src/igvc_cv/setup.cfg | 4 +++ src/igvc_cv/setup.py | 27 ++++++++++++++++++ 6 files changed, 101 insertions(+) create mode 100644 src/igvc_cv/igvc_cv/__init__.py create mode 100644 src/igvc_cv/igvc_cv/cv_node.py create mode 100644 src/igvc_cv/package.xml create mode 100644 src/igvc_cv/resource/igvc_cv create mode 100644 src/igvc_cv/setup.cfg create mode 100644 src/igvc_cv/setup.py 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..7494e3b3 --- /dev/null +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -0,0 +1,50 @@ +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 CVNode(Node): + def __init__(self): + super().__init__('cv_node') + + self.bridge = CvBridge() + + self.subscription = self.create_subscription(Image, 'image_raw', self.image_callback, 10) + + self.publisher = self.create_publisher(Image, 'image_processed', 10) + + self.get_logger().info("CV Node Started") + + def image_callback(self, msg): + + frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + blur = cv2.GaussianBlur(gray, (11, 11), 33) + edges = cv2.Canny(blur, 50, 150) + + lines = cv2.HoughLinesP(edges, 2, np.pi / 180, 50, minLineLength=30, maxLineGap=10) + + 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), 3) + + result = cv2.addWeighted(frame, 0.8, line_image, 1, 1) + + ros_image = self.bridge.cv2_to_imgmsg(result, 'bgr8') + self.publisher.publish(ros_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..daff1863 --- /dev/null +++ b/src/igvc_cv/package.xml @@ -0,0 +1,20 @@ + + + + igvc_cv + 0.0.0 + Computer Vision for the 2026 IGVC Robot + Camden Laursen-Carr + MIT + + rclpy + sensor_msgs + cv_bridge + + 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..effe8adc --- /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'], + 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', + ], + }, +) From 4171d10b48e8e745344b9d2c83b081ae84ac4b26 Mon Sep 17 00:00:00 2001 From: = Date: Mon, 23 Feb 2026 22:06:28 +0000 Subject: [PATCH 02/23] Updated Parameters --- src/igvc_cv/igvc_cv/cv_node.py | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 7494e3b3..46944a6f 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -19,25 +19,19 @@ def __init__(self): self.get_logger().info("CV Node Started") def image_callback(self, msg): - frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - blur = cv2.GaussianBlur(gray, (11, 11), 33) - edges = cv2.Canny(blur, 50, 150) - - lines = cv2.HoughLinesP(edges, 2, np.pi / 180, 50, minLineLength=30, maxLineGap=10) + blur = cv2.GaussianBlur(gray, (11, 11), 33, 33) + edges = cv2.Canny(blur, 100, 10) + lines = cv2.HoughLinesP(edges, 2, np.pi/180, 10, minLineLength=4, maxLineGap=5) 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), 3) - + cv2.line(line_image, (x1, y1), (x2, y2), (0, 255, 255), 10) result = cv2.addWeighted(frame, 0.8, line_image, 1, 1) - ros_image = self.bridge.cv2_to_imgmsg(result, 'bgr8') self.publisher.publish(ros_image) From a4d8658fcfd8c43d356fb15264ddf3a4033e52f1 Mon Sep 17 00:00:00 2001 From: = Date: Thu, 26 Feb 2026 00:10:29 +0000 Subject: [PATCH 03/23] Added arguments --- src/igvc_cv/igvc_cv/cv_node.py | 82 +++++++++++++++++++++++++++------- 1 file changed, 65 insertions(+), 17 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 46944a6f..221dcc0c 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -12,28 +12,76 @@ def __init__(self): self.bridge = CvBridge() - self.subscription = self.create_subscription(Image, 'image_raw', self.image_callback, 10) + self.subscription = self.create_subscription(Image, 'image_raw', self.process_image, 10) self.publisher = self.create_publisher(Image, 'image_processed', 10) + # 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) + self.get_logger().info("CV Node Started") - def image_callback(self, msg): - frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - blur = cv2.GaussianBlur(gray, (11, 11), 33, 33) - edges = cv2.Canny(blur, 100, 10) - - lines = cv2.HoughLinesP(edges, 2, np.pi/180, 10, minLineLength=4, maxLineGap=5) - 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), 10) - result = cv2.addWeighted(frame, 0.8, line_image, 1, 1) - ros_image = self.bridge.cv2_to_imgmsg(result, 'bgr8') - self.publisher.publish(ros_image) +def process_image(self, msg): + # Convert ROS image to OpenCV + frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + + # Convert to grayscale + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + # Ensure kernel size is odd (GaussianBlur requires odd dimensions) + k = self.blur_kernel_size + if k % 2 == 0: + k += 1 + + # Gaussian blur + blur = cv2.GaussianBlur( + gray, + (k, k), + self.blur_sigma_x, + self.blur_sigma_y + ) + + # Canny edge detection + edges = cv2.Canny( + blur, + self.canny_low, + self.canny_high + ) + + # Hough Line Transform + lines = cv2.HoughLinesP( + edges, + self.hough_rho, + self.hough_theta, + self.hough_threshold, + minLineLength=self.hough_min_line_length, + maxLineGap=self.hough_max_line_gap + ) + + 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), self.line_thickness) + result = cv2.addWeighted(frame, 0.8, line_image, 1, 1) + ros_image = self.bridge.cv2_to_imgmsg(result, 'bgr8') + self.publisher.publish(ros_image) def main(args=None): From bd902dec21543a79cdb71f462140a5c74011ac36 Mon Sep 17 00:00:00 2001 From: = Date: Thu, 26 Feb 2026 00:18:46 +0000 Subject: [PATCH 04/23] Improved Clarity --- src/igvc_cv/igvc_cv/cv_node.py | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 221dcc0c..547446e9 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -43,15 +43,10 @@ def process_image(self, msg): # Convert to grayscale gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - # Ensure kernel size is odd (GaussianBlur requires odd dimensions) - k = self.blur_kernel_size - if k % 2 == 0: - k += 1 - # Gaussian blur blur = cv2.GaussianBlur( gray, - (k, k), + (self.blur_kernel_size, self.blur_kernel_size), self.blur_sigma_x, self.blur_sigma_y ) From 761d8bd4a2cf9a42e458d18fd34291d2fc019962 Mon Sep 17 00:00:00 2001 From: eemukh Date: Wed, 25 Feb 2026 20:27:50 -0500 Subject: [PATCH 05/23] added paramters for Gaussian pyramid (INTENTIONALLY LEFT WITH DUMMY VALUES) --- src/igvc_cv/igvc_cv/cv_node.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 547446e9..3e2abb6a 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -31,6 +31,12 @@ def __init__(self): self.declare_parameter("hough_threshold", 10) self.declare_parameter("hough_min_line_length", 4) self.declare_parameter("hough_max_line_gap", 5) + + # Gaussian Pyramid Up-Sample + self.declare_parameter("layers", 4) + self.declare_parameter("pyrInput",[]) + self.declare_parameter("pyrOutput",[]) + self.declare_parameter("pyrSize".0) self.declare_parameter("line_thickness", 10) @@ -84,4 +90,4 @@ def main(args=None): node = CVNode() rclpy.spin(node) node.destroy_node() - rclpy.shutdown() \ No newline at end of file + rclpy.shutdown() From 0b38de0e365cc2f0e6694f445da8b9125b5f1248 Mon Sep 17 00:00:00 2001 From: h2re Date: Tue, 10 Mar 2026 21:08:21 +0000 Subject: [PATCH 06/23] Added Depth Sensing --- src/igvc_cv/igvc_cv/cv_node.py | 145 ++++++++++++++++++++------------- 1 file changed, 87 insertions(+), 58 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 3e2abb6a..892eb2c0 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -1,7 +1,9 @@ import rclpy from rclpy.node import Node -from sensor_msgs.msg import Image +from sensor_msgs.msg import Image, PointCloud2 from cv_bridge import CvBridge +from message_filters import ApproximateTimeSynchronizer, Subscriber +import sensor_msgs_py.point_cloud2 as pc2 import cv2 import numpy as np @@ -9,22 +11,17 @@ class CVNode(Node): def __init__(self): super().__init__('cv_node') - self.bridge = CvBridge() - self.subscription = self.create_subscription(Image, 'image_raw', self.process_image, 10) - - self.publisher = self.create_publisher(Image, 'image_processed', 10) - # 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) @@ -32,62 +29,94 @@ def __init__(self): self.declare_parameter("hough_min_line_length", 4) self.declare_parameter("hough_max_line_gap", 5) - # Gaussian Pyramid Up-Sample - self.declare_parameter("layers", 4) - self.declare_parameter("pyrInput",[]) - self.declare_parameter("pyrOutput",[]) - self.declare_parameter("pyrSize".0) - self.declare_parameter("line_thickness", 10) + + # Synchronised RGB + PointCloud subscribers + self.rgb_sub = Subscriber(self, Image, 'image_raw') + self.pc_sub = Subscriber(self, PointCloud2, 'depth/points') + 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) + + # ── Main callback ───────────────────────────────────────────────────── + def process(self, rgb_msg: Image, pc_msg: PointCloud2): + + # 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') + 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) - self.get_logger().info("CV Node Started") - -def process_image(self, msg): - # Convert ROS image to OpenCV - frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') - - # Convert to grayscale - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - - # Gaussian blur - blur = cv2.GaussianBlur( - gray, - (self.blur_kernel_size, self.blur_kernel_size), - self.blur_sigma_x, - self.blur_sigma_y - ) - - # Canny edge detection - edges = cv2.Canny( - blur, - self.canny_low, - self.canny_high - ) - - # Hough Line Transform - lines = cv2.HoughLinesP( - edges, - self.hough_rho, - self.hough_theta, - self.hough_threshold, - minLineLength=self.hough_min_line_length, - maxLineGap=self.hough_max_line_gap - ) - - 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), self.line_thickness) - result = cv2.addWeighted(frame, 0.8, line_image, 1, 1) - ros_image = self.bridge.cv2_to_imgmsg(result, 'bgr8') - self.publisher.publish(ros_image) + # 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) + # Read 3D points directly from point cloud + white_v, white_u = np.where(line_mask > 0) + + # Read the full cloud + cloud_array = pc2.read_points_numpy( + pc_msg, + field_names=("x", "y", "z"), + skip_nans=False, + reshape_organized_cloud=True # shape (HxWx3) + ) + + # Look up 3D coords for each white pixel directly + white_points = cloud_array[white_v, white_u] # Nx3 + + # No depth return) + valid = np.isfinite(white_points).all(axis=1) + white_points = white_points[valid] + + # 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) + def main(args=None): rclpy.init(args=args) node = CVNode() rclpy.spin(node) node.destroy_node() - rclpy.shutdown() + rclpy.shutdown() \ No newline at end of file From 2e4cc01b9018d0d9198c442b8479998ae77a46cd Mon Sep 17 00:00:00 2001 From: h2re Date: Tue, 10 Mar 2026 21:08:56 +0000 Subject: [PATCH 07/23] Updated Setup and Package --- src/igvc_cv/package.xml | 3 +++ src/igvc_cv/setup.py | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/igvc_cv/package.xml b/src/igvc_cv/package.xml index daff1863..0d121a7b 100644 --- a/src/igvc_cv/package.xml +++ b/src/igvc_cv/package.xml @@ -10,6 +10,9 @@ rclpy sensor_msgs cv_bridge + python3-opencv + python3-numpy + message_filters python3-pytest diff --git a/src/igvc_cv/setup.py b/src/igvc_cv/setup.py index effe8adc..71aa8243 100644 --- a/src/igvc_cv/setup.py +++ b/src/igvc_cv/setup.py @@ -11,7 +11,7 @@ ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ], - install_requires=['setuptools'], + install_requires=['setuptools', 'numpy', 'opencv-python', 'sensor-msgs-py'], zip_safe=True, maintainer='Camden Laursen-Carr', maintainer_email='laursc@rpi.edu', From ea3b59bd6e67b54850e63ae18adc5f2f773c601b Mon Sep 17 00:00:00 2001 From: Cam LC <124944268+H2re@users.noreply.github.com> Date: Tue, 10 Mar 2026 17:39:44 -0400 Subject: [PATCH 08/23] added small line changes --- src/igvc_cv/igvc_cv/cv_node.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 892eb2c0..e80898cb 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -7,7 +7,6 @@ import cv2 import numpy as np - class CVNode(Node): def __init__(self): super().__init__('cv_node') @@ -42,10 +41,8 @@ def __init__(self): self.sync.registerCallback(self.process) # ===== Publishers ===== self.image_pub = self.create_publisher(Image, 'image_processed', 10) - - # ── Main callback ───────────────────────────────────────────────────── + def process(self, rgb_msg: Image, pc_msg: PointCloud2): - # Fetch parameters blur_k = self.get_parameter("blur_kernel_size").value blur_sx = self.get_parameter("blur_sigma_x").value From d76a43cf18aa5a12d2e73f3fab9117962c8a2453 Mon Sep 17 00:00:00 2001 From: Vincent Borello Date: Thu, 12 Mar 2026 00:43:41 +0000 Subject: [PATCH 09/23] change topics to correct ones --- src/igvc_cv/igvc_cv/cv_node.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index e80898cb..0d9578ef 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -31,8 +31,8 @@ def __init__(self): self.declare_parameter("line_thickness", 10) # Synchronised RGB + PointCloud subscribers - self.rgb_sub = Subscriber(self, Image, 'image_raw') - self.pc_sub = Subscriber(self, PointCloud2, 'depth/points') + self.rgb_sub = Subscriber(self, Image, '/camera/camera/color/image_raw') + self.pc_sub = Subscriber(self, PointCloud2, '/camera/camera/depth/image_rect_raw') self.sync = ApproximateTimeSynchronizer( [self.rgb_sub, self.pc_sub], queue_size=10, @@ -43,6 +43,7 @@ def __init__(self): self.image_pub = self.create_publisher(Image, 'image_processed', 10) def process(self, rgb_msg: Image, pc_msg: PointCloud2): + 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 @@ -110,6 +111,7 @@ def process(self, rgb_msg: Image, pc_msg: PointCloud2): 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) From 6814eedc73e277e89ee66bb7a6908de86c7a4e83 Mon Sep 17 00:00:00 2001 From: h2re Date: Thu, 19 Mar 2026 00:03:56 +0000 Subject: [PATCH 10/23] Changed PointCloud to Depth Image --- src/igvc_cv/igvc_cv/cv_node.py | 29 ++++++++++------------------- 1 file changed, 10 insertions(+), 19 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 0d9578ef..6afe1bcf 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -1,9 +1,8 @@ import rclpy from rclpy.node import Node -from sensor_msgs.msg import Image, PointCloud2 +from sensor_msgs.msg import Image from cv_bridge import CvBridge from message_filters import ApproximateTimeSynchronizer, Subscriber -import sensor_msgs_py.point_cloud2 as pc2 import cv2 import numpy as np @@ -32,7 +31,7 @@ def __init__(self): # Synchronised RGB + PointCloud subscribers self.rgb_sub = Subscriber(self, Image, '/camera/camera/color/image_raw') - self.pc_sub = Subscriber(self, PointCloud2, '/camera/camera/depth/image_rect_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, @@ -42,7 +41,7 @@ def __init__(self): # ===== Publishers ===== self.image_pub = self.create_publisher(Image, 'image_processed', 10) - def process(self, rgb_msg: Image, pc_msg: PointCloud2): + 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 @@ -59,6 +58,7 @@ def process(self, rgb_msg: Image, pc_msg: PointCloud2): # 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 @@ -88,23 +88,14 @@ def process(self, rgb_msg: Image, pc_msg: PointCloud2): cv2.line(line_image, (x1, y1), (x2, y2), (0, 255, 255), thickness) cv2.line(line_mask, (x1, y1), (x2, y2), 255, thickness) - # Read 3D points directly from point cloud + # Look up depth for each white pixel directly white_v, white_u = np.where(line_mask > 0) - - # Read the full cloud - cloud_array = pc2.read_points_numpy( - pc_msg, - field_names=("x", "y", "z"), - skip_nans=False, - reshape_organized_cloud=True # shape (HxWx3) - ) + distances = depth_frame[white_v, white_u] - # Look up 3D coords for each white pixel directly - white_points = cloud_array[white_v, white_u] # Nx3 - - # No depth return) - valid = np.isfinite(white_points).all(axis=1) - white_points = white_points[valid] + valid = distances > 0 + white_u = white_u[valid] + white_v = white_v[valid] + distances = distances[valid] # Publish image result = cv2.addWeighted(frame, 0.8, line_image, 1.0, 1) From 2e4d115bd222b8269c111ac938431be603d99aea Mon Sep 17 00:00:00 2001 From: S Murokh Date: Fri, 3 Apr 2026 14:40:50 -0400 Subject: [PATCH 11/23] return a pointcloud (costmap should accept) rather than an image --- src/igvc_cv/igvc_cv/cv_node.py | 30 +++++++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 6afe1bcf..5777aad2 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -6,6 +6,10 @@ 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') @@ -92,17 +96,41 @@ def process(self, rgb_msg: Image, depth_msg: Image): white_v, white_u = np.where(line_mask > 0) distances = depth_frame[white_v, white_u] - valid = distances > 0 + valid = np.isfinite(distances) & distances > 0 white_u = white_u[valid] white_v = white_v[valid] distances = distances[valid] + #CAMERA PARAMETERS (replace with actual information through zed.get_camera_information()) + #also could be written as parameters in __init__ + fx = 700.0 + fy = 700.0 + cx = 640.0 + cy = 360.0 + + #points to 3D + Z = distances + X = (white_u - cx) * Z / fx + Y = (white_v - cy) * Z / 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) From 3eb91287573944ce77b392edbe0ba00e06d4ab9f Mon Sep 17 00:00:00 2001 From: S Murokh Date: Fri, 3 Apr 2026 16:54:00 -0400 Subject: [PATCH 12/23] add factory configuration values for camera --- src/igvc_cv/igvc_cv/cv_node.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 5777aad2..e456ea98 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -44,6 +44,12 @@ def __init__(self): 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()") @@ -101,17 +107,10 @@ def process(self, rgb_msg: Image, depth_msg: Image): white_v = white_v[valid] distances = distances[valid] - #CAMERA PARAMETERS (replace with actual information through zed.get_camera_information()) - #also could be written as parameters in __init__ - fx = 700.0 - fy = 700.0 - cx = 640.0 - cy = 360.0 - #points to 3D Z = distances - X = (white_u - cx) * Z / fx - Y = (white_v - cy) * Z / fy + 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 From 604d007759fde93ee9a165c460536eb953ed836e Mon Sep 17 00:00:00 2001 From: S Murokh Date: Fri, 10 Apr 2026 17:41:01 -0400 Subject: [PATCH 13/23] fix error --- src/igvc_cv/igvc_cv/cv_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index e456ea98..b1e61f64 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -102,7 +102,7 @@ def process(self, rgb_msg: Image, depth_msg: Image): white_v, white_u = np.where(line_mask > 0) distances = depth_frame[white_v, white_u] - valid = np.isfinite(distances) & distances > 0 + valid = distances > 0 white_u = white_u[valid] white_v = white_v[valid] distances = distances[valid] From 6c13cb5aa7a9d22e97aeeab4a5f4c25afa4501df Mon Sep 17 00:00:00 2001 From: Sasha Murokh Date: Tue, 21 Apr 2026 00:52:27 -0400 Subject: [PATCH 14/23] fix issues, should be outputting a valid point cloud. more testing to be done --- src/igvc_cv/igvc_cv/cv_node.py | 49 +++++++++++++++++++++++++--------- 1 file changed, 36 insertions(+), 13 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index b1e61f64..d103ec42 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -10,6 +10,9 @@ from std_msgs.msg import Header import sensor_msgs_py.point_cloud2 as pc2 +from rclpy.qos import QoSProfile, ReliabilityPolicy +from message_filters import Subscriber + class CVNode(Node): def __init__(self): super().__init__('cv_node') @@ -32,10 +35,30 @@ def __init__(self): 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') + qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + depth=10 + ) + + self.rgb_sub = Subscriber( + self, + Image, + '/camera/camera/color/image_raw', + qos_profile=qos + ) + + self.pc_sub = Subscriber( + self, + Image, + '/camera/camera/depth/image_rect_raw', + qos_profile=qos + ) + + self.sync = ApproximateTimeSynchronizer( [self.rgb_sub, self.pc_sub], queue_size=10, @@ -43,7 +66,10 @@ def __init__(self): ) self.sync.registerCallback(self.process) # ===== Publishers ===== - self.image_pub = self.create_publisher(Image, 'image_processed', 10) + + self.pc_pub = self.create_publisher(PointCloud2, 'cv_points', 10) + + self.get_logger().info("Node started") # Camera parameters based on factor calibration file self.fx = 1401.07 @@ -102,7 +128,7 @@ def process(self, rgb_msg: Image, depth_msg: Image): white_v, white_u = np.where(line_mask > 0) distances = depth_frame[white_v, white_u] - valid = distances > 0 + valid = np.isfinite(distances) & (distances > 0) white_u = white_u[valid] white_v = white_v[valid] distances = distances[valid] @@ -120,16 +146,13 @@ def process(self, rgb_msg: Image, depth_msg: Image): 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") - """ + self.get_logger().info(f"cloud: {pc2.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=True)[0][0]}") + self.get_logger().info(f"points shape: {points.shape}") + self.get_logger().info(f"num points: {len(points)}") + + self.pc_pub.publish(cloud_msg) + def main(args=None): rclpy.init(args=args) From 6dba2d826e259720525fe0732af7282f59071725 Mon Sep 17 00:00:00 2001 From: Gavin Date: Tue, 28 Apr 2026 02:12:44 +0000 Subject: [PATCH 15/23] small cleanup (imported subscriber twice) --- src/igvc_cv/igvc_cv/cv_node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index d103ec42..9073ef17 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -11,7 +11,6 @@ import sensor_msgs_py.point_cloud2 as pc2 from rclpy.qos import QoSProfile, ReliabilityPolicy -from message_filters import Subscriber class CVNode(Node): def __init__(self): From f8cdbd91b1259877134959879ebc20a36aaf703b Mon Sep 17 00:00:00 2001 From: Gavin Date: Tue, 28 Apr 2026 02:17:13 +0000 Subject: [PATCH 16/23] small syntax fix --- src/igvc_cv/igvc_cv/cv_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 9073ef17..620dfd59 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -146,7 +146,7 @@ def process(self, rgb_msg: Image, depth_msg: Image): cloud_msg = pc2.create_cloud_xyz32(header, points.tolist()) - self.get_logger().info(f"cloud: {pc2.read_points(cloud_msg, field_names=("x", "y", "z"), skip_nans=True)[0][0]}") + self.get_logger().info(f"cloud: {pc2.read_points(cloud_msg, field_names=('x', 'y', 'z'), skip_nans=True)[0][0]}") self.get_logger().info(f"points shape: {points.shape}") self.get_logger().info(f"num points: {len(points)}") From 2f22600052d7e2f9edeb9b481290db7c9847ceaf Mon Sep 17 00:00:00 2001 From: Gavin Date: Tue, 28 Apr 2026 02:19:13 +0000 Subject: [PATCH 17/23] added small todo to find frame id for camera --- src/igvc_cv/igvc_cv/cv_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 620dfd59..649ca010 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -142,7 +142,7 @@ def process(self, rgb_msg: Image, depth_msg: Image): header = Header() header.stamp = self.get_clock().now().to_msg() - header.frame_id = "" + header.frame_id = "" #TODO verify the camera frame id cloud_msg = pc2.create_cloud_xyz32(header, points.tolist()) From ba6d0056412330d41f0da1692b434566e271446c Mon Sep 17 00:00:00 2001 From: Gavin Date: Wed, 29 Apr 2026 17:32:59 +0000 Subject: [PATCH 18/23] Create left and right publishers, and helper function to construct goal path --- src/igvc_cv/igvc_cv/cv_node.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 649ca010..967d1e00 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -9,6 +9,8 @@ from sensor_msgs.msg import PointCloud2 from std_msgs.msg import Header import sensor_msgs_py.point_cloud2 as pc2 +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped from rclpy.qos import QoSProfile, ReliabilityPolicy @@ -67,6 +69,9 @@ def __init__(self): # ===== Publishers ===== self.pc_pub = self.create_publisher(PointCloud2, 'cv_points', 10) + # pubs for right and left lanes + self.leftlane = self.create_publisher(Path, '/left_boundary', 10) + self.rightlane = self.create_publisher(Path, '/right_boundary', 10) self.get_logger().info("Node started") @@ -151,6 +156,22 @@ def process(self, rgb_msg: Image, depth_msg: Image): self.get_logger().info(f"num points: {len(points)}") self.pc_pub.publish(cloud_msg) + + def points_to_path(self, points, frame_id): + path = Path() + path.header.stamp = self.get_clock().now().to_msg() + path.header.frame_id = frame_id + + for point in points: + pose = PoseStamped() + pose.header.stamp = path.header.stamp + pose.header.frame_id = frame_id + pose.pose.position.x = float(point[0]) + pose.pose.position.y = float(point[1]) + pose.pose.position.z = float(point[2]) + path.poses.append(pose) + + return path def main(args=None): From 80edc3b63232e3af1db830e9cfa360b70f7b6bfe Mon Sep 17 00:00:00 2001 From: Gavin Date: Wed, 29 Apr 2026 18:07:30 +0000 Subject: [PATCH 19/23] Split into left and right lanes for goal setting publishing --- src/igvc_cv/igvc_cv/cv_node.py | 30 +++++++++++++++++++++++++----- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 967d1e00..57b61346 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -34,10 +34,8 @@ def __init__(self): 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 qos = QoSProfile( @@ -59,7 +57,6 @@ def __init__(self): qos_profile=qos ) - self.sync = ApproximateTimeSynchronizer( [self.rgb_sub, self.pc_sub], queue_size=10, @@ -73,14 +70,15 @@ def __init__(self): self.leftlane = self.create_publisher(Path, '/left_boundary', 10) self.rightlane = self.create_publisher(Path, '/right_boundary', 10) - self.get_logger().info("Node started") - + # Camera parameters based on factor calibration file self.fx = 1401.07 self.fy = 1401.07 self.cx = 1062.32 self.cy = 634.124 + self.get_logger().info("Node started") + def process(self, rgb_msg: Image, depth_msg: Image): self.get_logger().info("Called Process()") # Fetch parameters @@ -151,6 +149,27 @@ def process(self, rgb_msg: Image, depth_msg: Image): cloud_msg = pc2.create_cloud_xyz32(header, points.tolist()) + #gather forward points + base_x = Z + #gather left and right points + base_y = -X + base_z = np.zeros_like(Z) + + base_points = np.vstack((base_x, base_y, base_z)).T + + #split left and right points + left_points = base_points[base_points[:,1] > 0] + right_points = base_points[base_points[:, 1] < 0] + + lane_frame_id = "base_footprint" #TODO find this out for sure + + left_path = self.points_to_path(left_points, lane_frame_id) + right_path = self.points_to_path(right_points, lane_frame_id) + + self.leftlane.publish(left_path) + self.rightlane.publish(right_path) + + self.get_logger().info(f"cloud: {pc2.read_points(cloud_msg, field_names=('x', 'y', 'z'), skip_nans=True)[0][0]}") self.get_logger().info(f"points shape: {points.shape}") self.get_logger().info(f"num points: {len(points)}") @@ -169,6 +188,7 @@ def points_to_path(self, points, frame_id): pose.pose.position.x = float(point[0]) pose.pose.position.y = float(point[1]) pose.pose.position.z = float(point[2]) + pose.pose.orientation.w = 1.0 #double check path.poses.append(pose) return path From 957b0ac7e3c74b693a218f158ba7dc7b9aaae476 Mon Sep 17 00:00:00 2001 From: Gavin Date: Wed, 29 Apr 2026 18:30:56 +0000 Subject: [PATCH 20/23] added testing parameter to start developing a local test of left and right lane parsing --- src/igvc_cv/igvc_cv/cv_node.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 57b61346..d154b66a 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -36,6 +36,12 @@ def __init__(self): self.declare_parameter("hough_max_line_gap", 5) self.declare_parameter("line_thickness", 10) + # Test mode param + self.declare_parameter("test", False) + + if self.get_parameter("test_mode").value: + self.get_logger().info("Running in test mode with fake images") + self.create_timer(1.0, self.run_fake_test) # Synchronised RGB + PointCloud subscribers qos = QoSProfile( @@ -170,7 +176,12 @@ def process(self, rgb_msg: Image, depth_msg: Image): self.rightlane.publish(right_path) - self.get_logger().info(f"cloud: {pc2.read_points(cloud_msg, field_names=('x', 'y', 'z'), skip_nans=True)[0][0]}") + # self.get_logger().info(f"cloud: {pc2.read_points(cloud_msg, field_names=('x', 'y', 'z'), skip_nans=True)[0][0]}") + if len(points) > 0: + self.get_logger().info(f"sample point: {points[0]}") + else: + self.get_logger().warn("No valid points detected") + self.get_logger().info(f"points shape: {points.shape}") self.get_logger().info(f"num points: {len(points)}") From c2187b6062b5032ce7c8cc63058a1a1448f88eae Mon Sep 17 00:00:00 2001 From: Gavin Date: Wed, 29 Apr 2026 19:17:23 +0000 Subject: [PATCH 21/23] added basic testing helper, file now runs, can be tested with camera soon when frame id and connectivity is verified --- src/igvc_cv/igvc_cv/cv_node.py | 33 ++++++++++++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index d154b66a..18881128 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -39,7 +39,7 @@ def __init__(self): # Test mode param self.declare_parameter("test", False) - if self.get_parameter("test_mode").value: + if self.get_parameter("test").value: self.get_logger().info("Running in test mode with fake images") self.create_timer(1.0, self.run_fake_test) @@ -151,7 +151,7 @@ def process(self, rgb_msg: Image, depth_msg: Image): header = Header() header.stamp = self.get_clock().now().to_msg() - header.frame_id = "" #TODO verify the camera frame id + header.frame_id = "camera_color_optical_frame" #TODO chang to real frame id cloud_msg = pc2.create_cloud_xyz32(header, points.tolist()) @@ -186,6 +186,30 @@ def process(self, rgb_msg: Image, depth_msg: Image): self.get_logger().info(f"num points: {len(points)}") self.pc_pub.publish(cloud_msg) + def run_fake_test(self): + h, w = 1080, 1920 + + # Fake black camera image + frame = np.zeros((h, w, 3), dtype=np.uint8) + + # Fake white lane lines + cv2.line(frame, (600, 1000), (900, 300), (255, 255, 255), 20) + cv2.line(frame, (1320, 1000), (1020, 300), (255, 255, 255), 20) + + # Fake depth image: everything is 2 meters away + depth_frame = np.full((h, w), 2.0, dtype=np.float32) + + stamp = self.get_clock().now().to_msg() + + rgb_msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') + rgb_msg.header.stamp = stamp + rgb_msg.header.frame_id = "camera_color_optical_frame" + + depth_msg = self.bridge.cv2_to_imgmsg(depth_frame, encoding='32FC1') + depth_msg.header.stamp = stamp + depth_msg.header.frame_id = "camera_color_optical_frame" + + self.process(rgb_msg, depth_msg) def points_to_path(self, points, frame_id): path = Path() @@ -210,4 +234,7 @@ def main(args=None): node = CVNode() rclpy.spin(node) node.destroy_node() - rclpy.shutdown() \ No newline at end of file + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From cfb3e6852df7f897e7d40af07c439c79248698c1 Mon Sep 17 00:00:00 2001 From: Gavin Date: Wed, 29 Apr 2026 20:08:30 +0000 Subject: [PATCH 22/23] added basic debugging print for left and rightlanes --- src/igvc_cv/igvc_cv/cv_node.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 18881128..01dddc8c 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -184,6 +184,9 @@ def process(self, rgb_msg: Image, depth_msg: Image): self.get_logger().info(f"points shape: {points.shape}") self.get_logger().info(f"num points: {len(points)}") + self.get_logger().info( + f"left path: {len(left_path.poses)}, right path: {len(right_path.poses)}" + ) self.pc_pub.publish(cloud_msg) def run_fake_test(self): From b0a689ae72e6e6e646ad6c7d3fab6e9d5bdcaf88 Mon Sep 17 00:00:00 2001 From: Gavin Date: Wed, 29 Apr 2026 20:27:28 +0000 Subject: [PATCH 23/23] added code to sort the points by distance --- src/igvc_cv/igvc_cv/cv_node.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/igvc_cv/igvc_cv/cv_node.py b/src/igvc_cv/igvc_cv/cv_node.py index 01dddc8c..10157319 100644 --- a/src/igvc_cv/igvc_cv/cv_node.py +++ b/src/igvc_cv/igvc_cv/cv_node.py @@ -219,6 +219,10 @@ def points_to_path(self, points, frame_id): path.header.stamp = self.get_clock().now().to_msg() path.header.frame_id = frame_id + # sort by forward distance + if len(points) > 0: + points = points[points[:, 0].argsort()] + for point in points: pose = PoseStamped() pose.header.stamp = path.header.stamp