From 80f21cc6160575e9085c186b0e84c153f020aad4 Mon Sep 17 00:00:00 2001 From: Adam Yaj Date: Tue, 10 Feb 2026 18:30:42 -0600 Subject: [PATCH 01/21] Implement a qt user interface to view topic image streams --- .../camera_interface/__init__.py | 0 .../camera_interface/qt_user_interface.py | 329 ++++++++++++++++++ .../camera_interface/webcam.py | 54 +++ src/camera_interface/package.xml | 22 ++ .../resource/camera_interface | 0 src/camera_interface/setup.cfg | 4 + src/camera_interface/setup.py | 33 ++ src/camera_interface/test/test_copyright.py | 25 ++ src/camera_interface/test/test_flake8.py | 25 ++ src/camera_interface/test/test_pep257.py | 23 ++ 10 files changed, 515 insertions(+) create mode 100644 src/camera_interface/camera_interface/__init__.py create mode 100644 src/camera_interface/camera_interface/qt_user_interface.py create mode 100644 src/camera_interface/camera_interface/webcam.py create mode 100644 src/camera_interface/package.xml create mode 100644 src/camera_interface/resource/camera_interface create mode 100644 src/camera_interface/setup.cfg create mode 100644 src/camera_interface/setup.py create mode 100644 src/camera_interface/test/test_copyright.py create mode 100644 src/camera_interface/test/test_flake8.py create mode 100644 src/camera_interface/test/test_pep257.py diff --git a/src/camera_interface/camera_interface/__init__.py b/src/camera_interface/camera_interface/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/camera_interface/camera_interface/qt_user_interface.py b/src/camera_interface/camera_interface/qt_user_interface.py new file mode 100644 index 00000000..10cfb9fe --- /dev/null +++ b/src/camera_interface/camera_interface/qt_user_interface.py @@ -0,0 +1,329 @@ +import sys +import threading +import time +import math + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +from sensor_msgs.msg import Image +from cv_bridge import CvBridge + +import cv2 +from PySide6.QtWidgets import ( + QApplication, QMainWindow, QLabel, QPushButton, + QVBoxLayout, QHBoxLayout, QWidget, + QListWidget, QListWidgetItem, + QGridLayout, QSizePolicy +) +from PySide6.QtGui import QImage, QPixmap +from PySide6.QtCore import Qt, Signal, QObject + +# -------------------- Dark Theme -------------------- + +dark_stylesheet = """ +QWidget { + background-color: #121212; + color: #e0e0e0; +} +QPushButton { + background-color: #1f1f1f; + border: 1px solid #333; + padding: 6px; +} +QListWidget { + background-color: #1c1c1c; +} +""" + +# -------------------- Qt Signal -------------------- + +class ImageSignal(QObject): + image_ready = Signal(object) + +# -------------------- ROS Node -------------------- + +class UserInterfaceNode(Node): + def __init__(self): + super().__init__('multi_camera_qt_interface') + self.bridge = CvBridge() + self.camera_subscriptions = {} + self.image_signals = {} + self.last_frame = {} + self.max_fps = 30.0 + + def subscribe(self, topic): + if topic in self.camera_subscriptions: + return + + qos = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1 + ) + + self.last_frame[topic] = 0.0 + + self.camera_subscriptions[topic] = self.create_subscription( + Image, + topic, + lambda msg, t=topic: self.callback(msg, t), + qos + ) + + def unsubscribe(self, topic): + if topic in self.camera_subscriptions: + self.destroy_subscription(self.camera_subscriptions[topic]) + del self.camera_subscriptions[topic] + del self.last_frame[topic] + del self.image_signals[topic] + + def callback(self, msg, topic): + # Topic removed while callback was queued + if topic not in self.image_signals: + return + + now = time.time() + if now - self.last_frame.get(topic, 0.0) < 1.0 / self.max_fps: + return + + self.last_frame[topic] = now + + cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') + + signal = self.image_signals.get(topic) + if signal is not None: + signal.image_ready.emit(cv_img) + +# -------------------- Camera Widget -------------------- + +class CameraWidget(QWidget): + def __init__(self, topic): + super().__init__() + self.topic = topic + + self.label = QLabel("Waiting for image...") + self.label.setAlignment(Qt.AlignCenter) + self.label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Ignored) + self.label.setMinimumSize(1, 1) + + title = QLabel(topic) + title.setAlignment(Qt.AlignCenter) + title.setFixedHeight(18) + title.setStyleSheet("font-size:10px; border-top:1px solid #333") + + # ---- Image container ---- + image_container = QWidget() + image_layout = QVBoxLayout() + image_layout.setContentsMargins(0, 0, 0, 0) + image_layout.addWidget(self.label) + # image_layout.addWidget(title, 0) + image_container.setLayout(image_layout) + + image_container.setSizePolicy( + QSizePolicy.Expanding, + QSizePolicy.Ignored + ) + + # ---- Main layout ---- + layout = QVBoxLayout() + layout.setContentsMargins(0, 0, 0, 0) + layout.setSpacing(0) + + layout.addWidget(title, 1) # stays attached + layout.addWidget(image_container, 0) # stretches + + self.setLayout(layout) + + def update_image(self, cv_img): + self.last_frame = cv_img # cache frame + + h, w, ch = cv_img.shape + qimg = QImage(cv_img.data, w, h, ch * w, QImage.Format_RGB888) + pix = QPixmap.fromImage(qimg) + + self.label.setPixmap( + pix.scaled( + self.label.size(), + Qt.KeepAspectRatio, + Qt.SmoothTransformation + ) + ) + + def resizeEvent(self, event): + if hasattr(self, "last_frame"): + self.update_image(self.last_frame) + +# -------------------- Main Window -------------------- + +class MultiCameraWindow(QMainWindow): + def __init__(self, ros_node): + super().__init__() + self.setWindowTitle("ROS2 Multi-Camera Viewer") + self.ros_node = ros_node + + self.camera_widgets = {} + + main_widget = QWidget() + main_layout = QHBoxLayout() + + # ---- Sidebar ---- + side_layout = QVBoxLayout() + self.refresh_btn = QPushButton("Refresh Topics") + self.refresh_btn.clicked.connect(self.refresh_topics) + + self.topic_list = QListWidget() + self.topic_list.itemChanged.connect(self.topic_toggled) + + side_layout.addWidget(self.topic_list) + side_layout.addWidget(self.refresh_btn) + + # ---- Grid Area ---- + self.grid_widget = QWidget() + self.grid_layout = QGridLayout() + self.grid_widget.setLayout(self.grid_layout) + self.grid_widget.setSizePolicy( + QSizePolicy.Expanding, + QSizePolicy.Expanding + ) + + + main_layout.addLayout(side_layout) + main_layout.addWidget(self.grid_widget, 1) + + main_widget.setLayout(main_layout) + self.setCentralWidget(main_widget) + + self.refresh_topics() + + # -------------------- + + def refresh_topics(self): + self.topic_list.blockSignals(True) + self.topic_list.clear() + + topics = self.ros_node.get_topic_names_and_types() + for name, types in topics: + if "sensor_msgs/msg/Image" in types: + item = QListWidgetItem(name) + item.setFlags(item.flags() | Qt.ItemIsUserCheckable) + item.setCheckState(Qt.Unchecked) + self.topic_list.addItem(item) + + self.topic_list.blockSignals(False) + + # -------------------- + + def topic_toggled(self, item): + topic = item.text() + checked = item.checkState() == Qt.Checked + + if checked: + self.add_camera(topic) + else: + self.remove_camera(topic) + + # -------------------- + + def add_camera(self, topic): + if topic in self.camera_widgets: + return + + widget = CameraWidget(topic) + signal = ImageSignal() + signal.image_ready.connect(widget.update_image) + + self.ros_node.image_signals[topic] = signal + self.ros_node.subscribe(topic) + + self.camera_widgets[topic] = widget + self.rebuild_grid() + + def remove_camera(self, topic): + if topic not in self.camera_widgets: + return + + self.ros_node.unsubscribe(topic) + + widget = self.camera_widgets[topic] + widget.setParent(None) + widget.deleteLater() + + del self.camera_widgets[topic] + self.rebuild_grid() + + + # -------------------- + + def rebuild_grid(self): + # Clear previous stretches to prevent "dead space" + for i in range(self.grid_layout.rowCount()): + self.grid_layout.setRowStretch(i, 0) + for i in range(self.grid_layout.columnCount()): + self.grid_layout.setColumnStretch(i, 0) + + # Properly clear the layout + while self.grid_layout.count(): + item = self.grid_layout.takeAt(0) + if item.widget(): + item.widget().hide() + + widgets = list(self.camera_widgets.values()) + count = len(widgets) + + if count == 0: + return + + cols = math.ceil(math.sqrt(count)) + rows = math.ceil(count / cols) + + index = 0 + for r in range(rows): + for c in range(cols): + if index >= count: + break + + widget = widgets[index] + self.grid_layout.addWidget(widget, r, c) + + # Ensure it's visible and active + widget.setVisible(True) + widget.update() # Triggers a repaint of the camera frame + + index += 1 + + for r in range(rows): + self.grid_layout.setRowStretch(r, 1) + for c in range(cols): + self.grid_layout.setColumnStretch(c, 1) + + self.grid_layout.setContentsMargins(0, 0, 0, 0) + self.grid_layout.setSpacing(1) + + + +# -------------------- Main -------------------- + +def main(): + rclpy.init() + + app = QApplication(sys.argv) + app.setStyleSheet(dark_stylesheet) + + ros_node = UserInterfaceNode() + + ros_thread = threading.Thread( + target=rclpy.spin, + args=(ros_node,), + daemon=True + ) + ros_thread.start() + + win = MultiCameraWindow(ros_node) + # win.resize(1200, 800) + win.show() + + sys.exit(app.exec()) + +if __name__ == "__main__": + main() diff --git a/src/camera_interface/camera_interface/webcam.py b/src/camera_interface/camera_interface/webcam.py new file mode 100644 index 00000000..7b259749 --- /dev/null +++ b/src/camera_interface/camera_interface/webcam.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image + +import cv2 +from cv_bridge import CvBridge + +class WebcamPublisher(Node): + def __init__(self): + super().__init__('webcam_node') + self.publisher_ = self.create_publisher(Image, '/webcam/image_raw', 10) + self.publisher1_ = self.create_publisher(Image, '/webcam/image_raw_1', 10) + self.publisher2_ = self.create_publisher(Image, '/webcam/image_raw_2', 10) + self.publisher3_ = self.create_publisher(Image, '/webcam/image_raw_3', 10) + self.timer = self.create_timer(0.01, self.timer_callback) + self.bridge = CvBridge() + self.cap = cv2.VideoCapture(0) + + def timer_callback(self): + msg = Image() + + if not self.cap.isOpened(): + self.get_logger().error('Error: Could not open webcam.') + self.destroy_node() + + success, frame = self.cap.read() + + # If frame is not read successfully, break the loop + if not success: + self.get_logger().warn("Error: Could not read frame.") + return + + msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') + + self.get_logger().info('Publishing: laptop webcam frame') + + self.publisher_.publish(msg) + self.publisher1_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, 0), encoding='bgr8')) + self.publisher2_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, 1), encoding='bgr8')) + self.publisher3_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, -1), encoding='bgr8')) + +def main(args=None): + rclpy.init(args=args) + webcam_publisher = WebcamPublisher() + + rclpy.spin(webcam_publisher) + webcam_publisher.destroy_node() + + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/camera_interface/package.xml b/src/camera_interface/package.xml new file mode 100644 index 00000000..3fd290a8 --- /dev/null +++ b/src/camera_interface/package.xml @@ -0,0 +1,22 @@ + + + + camera_interface + 0.0.0 + TODO: Package description + adam + TODO: License declaration + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + tkinter + + + ament_python + + diff --git a/src/camera_interface/resource/camera_interface b/src/camera_interface/resource/camera_interface new file mode 100644 index 00000000..e69de29b diff --git a/src/camera_interface/setup.cfg b/src/camera_interface/setup.cfg new file mode 100644 index 00000000..115fcfc4 --- /dev/null +++ b/src/camera_interface/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/camera_interface +[install] +install_scripts=$base/lib/camera_interface diff --git a/src/camera_interface/setup.py b/src/camera_interface/setup.py new file mode 100644 index 00000000..5a74f209 --- /dev/null +++ b/src/camera_interface/setup.py @@ -0,0 +1,33 @@ +from setuptools import find_packages, setup + +package_name = 'camera_interface' + +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='adam', + maintainer_email='hwvxyeej@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'camera = camera_interface.camera:main', + 'user_interface = camera_interface.user_interface:main', + 'qt_user_interface = camera_interface.qt_user_interface:main', + 'webcam = camera_interface.webcam:main' + ], + }, +) diff --git a/src/camera_interface/test/test_copyright.py b/src/camera_interface/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/camera_interface/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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/camera_interface/test/test_flake8.py b/src/camera_interface/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/camera_interface/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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/camera_interface/test/test_pep257.py b/src/camera_interface/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/camera_interface/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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' From 123b3abd8e85e2c66429aa18d7a7e862ae3b8635 Mon Sep 17 00:00:00 2001 From: Adam Yaj Date: Tue, 10 Feb 2026 18:32:40 -0600 Subject: [PATCH 02/21] Remove non-existant files from console_scripts of camera_interface --- src/camera_interface/setup.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/camera_interface/setup.py b/src/camera_interface/setup.py index 5a74f209..e1a59aaa 100644 --- a/src/camera_interface/setup.py +++ b/src/camera_interface/setup.py @@ -24,8 +24,6 @@ }, entry_points={ 'console_scripts': [ - 'camera = camera_interface.camera:main', - 'user_interface = camera_interface.user_interface:main', 'qt_user_interface = camera_interface.qt_user_interface:main', 'webcam = camera_interface.webcam:main' ], From cac8ec0246543c6978acd91bfb8471d73c50bab2 Mon Sep 17 00:00:00 2001 From: alex berg Date: Tue, 17 Feb 2026 18:53:09 -0600 Subject: [PATCH 03/21] minimium viable product --- README.md | 2 + docker/Dockerfile.umn | 5 +- .../camera_interface/qt_user_interface.py | 395 +++++++++--------- .../camera_interface/webcam.py | 163 ++++++-- 4 files changed, 323 insertions(+), 242 deletions(-) diff --git a/README.md b/README.md index 20ef4533..1f42efab 100644 --- a/README.md +++ b/README.md @@ -99,6 +99,8 @@ devcontainer build --push true --workspace-folder . --platform="linux/amd64,linu
How to Run Inside the ISAAC ROS Container on Linux/Jetson
+** FOR MORE INFO ON UPDATING AND USING DOCKER STUFF, SEE THE README INSIDE THE `docker` directory ** + First, you will need to log in to Nvidia NGC and get an API Key here: https://org.ngc.nvidia.com/setup Then install Nvidia ngc CLI and make sure it is present in path: https://org.ngc.nvidia.com/setup/installers/cli diff --git a/docker/Dockerfile.umn b/docker/Dockerfile.umn index ff532cef..9543d6c5 100644 --- a/docker/Dockerfile.umn +++ b/docker/Dockerfile.umn @@ -43,11 +43,14 @@ RUN --mount=type=cache,target=/var/cache/apt apt-get update && apt-get install libudev-dev \ libusb-1.0-0-dev \ libhidapi-libusb0 \ + libxcb-cursor0 \ && rm -rf /var/lib/apt/lists/* RUN python3 -m pip install -U \ pyvista \ - streamdeck + streamdeck \ + PySide6 \ + av RUN echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="0fd9", MODE="0666"' | sudo tee /etc/udev/rules.d/99-streamdeck.rules diff --git a/src/camera_interface/camera_interface/qt_user_interface.py b/src/camera_interface/camera_interface/qt_user_interface.py index 10cfb9fe..d224148d 100644 --- a/src/camera_interface/camera_interface/qt_user_interface.py +++ b/src/camera_interface/camera_interface/qt_user_interface.py @@ -2,19 +2,20 @@ import threading import time import math +import numpy as np +import av # PyAV import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy -from sensor_msgs.msg import Image +from sensor_msgs.msg import Image, CompressedImage from cv_bridge import CvBridge -import cv2 from PySide6.QtWidgets import ( QApplication, QMainWindow, QLabel, QPushButton, QVBoxLayout, QHBoxLayout, QWidget, QListWidget, QListWidgetItem, - QGridLayout, QSizePolicy + QGridLayout, QSizePolicy # <--- Added QSizePolicy import ) from PySide6.QtGui import QImage, QPixmap from PySide6.QtCore import Qt, Signal, QObject @@ -22,38 +23,54 @@ # -------------------- Dark Theme -------------------- dark_stylesheet = """ -QWidget { - background-color: #121212; - color: #e0e0e0; -} -QPushButton { - background-color: #1f1f1f; - border: 1px solid #333; - padding: 6px; -} -QListWidget { - background-color: #1c1c1c; -} +QWidget { background-color: #121212; color: #e0e0e0; } +QPushButton { background-color: #1f1f1f; border: 1px solid #333; padding: 6px; } +QPushButton:hover { background-color: #333; } +QListWidget { background-color: #1c1c1c; border: 1px solid #333; } """ -# -------------------- Qt Signal -------------------- +# -------------------- Decoding Logic -------------------- class ImageSignal(QObject): image_ready = Signal(object) +class StreamDecoder: + """Stateful decoder using PyAV for AV1/HEVC/H264 streams.""" + def __init__(self, codec_name='av1'): + try: + self.codec = av.CodecContext.create(codec_name, 'r') + except Exception as e: + print(f"Error creating codec {codec_name}: {e}") + self.codec = None + + def decode(self, binary_data): + if not self.codec: + return None + + try: + packets = self.codec.parse(binary_data) + for packet in packets: + frames = self.codec.decode(packet) + for frame in frames: + return frame.to_ndarray(format='rgb24') + except Exception as e: + print(f"Decode error: {e}") + return None + # -------------------- ROS Node -------------------- class UserInterfaceNode(Node): def __init__(self): super().__init__('multi_camera_qt_interface') self.bridge = CvBridge() - self.camera_subscriptions = {} + self.subscriptions_map = {} self.image_signals = {} - self.last_frame = {} - self.max_fps = 30.0 + self.decoders = {} + self.last_frame_time = {} + self.max_fps = 60.0 - def subscribe(self, topic): - if topic in self.camera_subscriptions: + def subscribe(self, topic, msg_type_str): + if topic in self.subscriptions_map: return qos = QoSProfile( @@ -61,269 +78,235 @@ def subscribe(self, topic): history=QoSHistoryPolicy.KEEP_LAST, depth=1 ) + self.last_frame_time[topic] = 0.0 - self.last_frame[topic] = 0.0 + # Map string type to class type + if 'CompressedImage' in str(msg_type_str): + msg_type = CompressedImage + else: + msg_type = Image - self.camera_subscriptions[topic] = self.create_subscription( - Image, + self.subscriptions_map[topic] = self.create_subscription( + msg_type, topic, lambda msg, t=topic: self.callback(msg, t), qos ) def unsubscribe(self, topic): - if topic in self.camera_subscriptions: - self.destroy_subscription(self.camera_subscriptions[topic]) - del self.camera_subscriptions[topic] - del self.last_frame[topic] - del self.image_signals[topic] + if topic in self.subscriptions_map: + self.destroy_subscription(self.subscriptions_map[topic]) + del self.subscriptions_map[topic] + del self.last_frame_time[topic] + if topic in self.decoders: + del self.decoders[topic] def callback(self, msg, topic): - # Topic removed while callback was queued if topic not in self.image_signals: return now = time.time() - if now - self.last_frame.get(topic, 0.0) < 1.0 / self.max_fps: + if now - self.last_frame_time.get(topic, 0.0) < 1.0 / self.max_fps: return - - self.last_frame[topic] = now - - cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') - - signal = self.image_signals.get(topic) - if signal is not None: - signal.image_ready.emit(cv_img) - -# -------------------- Camera Widget -------------------- + self.last_frame_time[topic] = now + + cv_img = None + try: + if isinstance(msg, CompressedImage): + if topic not in self.decoders: + fmt = msg.format.lower() + codec = 'hevc' if 'hevc' in fmt else 'av1' if 'av1' in fmt else 'h264' + self.decoders[topic] = StreamDecoder(codec) + + cv_img = self.decoders[topic].decode(bytes(msg.data)) + + elif isinstance(msg, Image): + if "av1" in msg.encoding.lower(): + if topic not in self.decoders: + self.decoders[topic] = StreamDecoder('av1') + cv_img = self.decoders[topic].decode(bytes(msg.data)) + else: + cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') + + if cv_img is not None: + signal = self.image_signals.get(topic) + if signal: + signal.image_ready.emit(cv_img) + + except Exception as e: + self.get_logger().error(f"Processing error on {topic}: {e}") + +# -------------------- UI Components -------------------- class CameraWidget(QWidget): def __init__(self, topic): super().__init__() self.topic = topic + self.last_frame = None - self.label = QLabel("Waiting for image...") + self.label = QLabel("Waiting for Stream...") self.label.setAlignment(Qt.AlignCenter) - self.label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Ignored) - self.label.setMinimumSize(1, 1) - + self.label.setStyleSheet("background-color: #000; color: #555;") + + # FIX 1: Use QSizePolicy instead of QWidget.SizePolicy + self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) + title = QLabel(topic) title.setAlignment(Qt.AlignCenter) - title.setFixedHeight(18) - title.setStyleSheet("font-size:10px; border-top:1px solid #333") - - # ---- Image container ---- - image_container = QWidget() - image_layout = QVBoxLayout() - image_layout.setContentsMargins(0, 0, 0, 0) - image_layout.addWidget(self.label) - # image_layout.addWidget(title, 0) - image_container.setLayout(image_layout) - - image_container.setSizePolicy( - QSizePolicy.Expanding, - QSizePolicy.Ignored - ) - - # ---- Main layout ---- - layout = QVBoxLayout() + title.setStyleSheet("font-size: 10px; color: #888; background: #1a1a1a; padding: 2px;") + + layout = QVBoxLayout(self) layout.setContentsMargins(0, 0, 0, 0) layout.setSpacing(0) - - layout.addWidget(title, 1) # stays attached - layout.addWidget(image_container, 0) # stretches - - self.setLayout(layout) + layout.addWidget(self.label, 1) + layout.addWidget(title) def update_image(self, cv_img): - self.last_frame = cv_img # cache frame - + self.last_frame = cv_img h, w, ch = cv_img.shape - qimg = QImage(cv_img.data, w, h, ch * w, QImage.Format_RGB888) + bytes_per_line = ch * w + + qimg = QImage(cv_img.data, w, h, bytes_per_line, QImage.Format_RGB888).copy() + pix = QPixmap.fromImage(qimg) - - self.label.setPixmap( - pix.scaled( - self.label.size(), - Qt.KeepAspectRatio, - Qt.SmoothTransformation - ) - ) + self.label.setPixmap(pix.scaled( + self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation + )) def resizeEvent(self, event): - if hasattr(self, "last_frame"): + if self.last_frame is not None: self.update_image(self.last_frame) - -# -------------------- Main Window -------------------- + super().resizeEvent(event) class MultiCameraWindow(QMainWindow): def __init__(self, ros_node): super().__init__() - self.setWindowTitle("ROS2 Multi-Camera Viewer") self.ros_node = ros_node - self.camera_widgets = {} + self.setWindowTitle("ROS2 Stream Viewer") main_widget = QWidget() - main_layout = QHBoxLayout() - - # ---- Sidebar ---- - side_layout = QVBoxLayout() - self.refresh_btn = QPushButton("Refresh Topics") - self.refresh_btn.clicked.connect(self.refresh_topics) - + self.layout = QHBoxLayout(main_widget) + + side_panel = QWidget() + side_layout = QVBoxLayout(side_panel) self.topic_list = QListWidget() self.topic_list.itemChanged.connect(self.topic_toggled) - + + btn_refresh = QPushButton("Refresh Topics") + btn_refresh.clicked.connect(self.refresh_topics) + + side_layout.addWidget(QLabel("Available Topics:")) side_layout.addWidget(self.topic_list) - side_layout.addWidget(self.refresh_btn) - - # ---- Grid Area ---- + side_layout.addWidget(btn_refresh) + self.grid_widget = QWidget() - self.grid_layout = QGridLayout() - self.grid_widget.setLayout(self.grid_layout) - self.grid_widget.setSizePolicy( - QSizePolicy.Expanding, - QSizePolicy.Expanding - ) - - - main_layout.addLayout(side_layout) - main_layout.addWidget(self.grid_widget, 1) - - main_widget.setLayout(main_layout) + self.grid_layout = QGridLayout(self.grid_widget) + self.grid_layout.setContentsMargins(0,0,0,0) + + self.layout.addWidget(side_panel, 1) + self.layout.addWidget(self.grid_widget, 4) self.setCentralWidget(main_widget) - + self.refresh_topics() - # -------------------- - def refresh_topics(self): self.topic_list.blockSignals(True) - self.topic_list.clear() + checked = {self.topic_list.item(i).text() for i in range(self.topic_list.count()) + if self.topic_list.item(i).checkState() == Qt.Checked} - topics = self.ros_node.get_topic_names_and_types() - for name, types in topics: - if "sensor_msgs/msg/Image" in types: + self.topic_list.clear() + + try: + topic_names_and_types = self.ros_node.get_topic_names_and_types() + except Exception as e: + print(f"Error fetching topics: {e}") + topic_names_and_types = [] + + for name, types in topic_names_and_types: + # Check for relevant message types + if any(t in ['sensor_msgs/msg/Image', 'sensor_msgs/msg/CompressedImage'] for t in types): item = QListWidgetItem(name) item.setFlags(item.flags() | Qt.ItemIsUserCheckable) - item.setCheckState(Qt.Unchecked) + item.setCheckState(Qt.Checked if name in checked else Qt.Unchecked) + # Store the type string for later use + item.setData(Qt.UserRole, types[0]) self.topic_list.addItem(item) - + self.topic_list.blockSignals(False) - # -------------------- - def topic_toggled(self, item): topic = item.text() - checked = item.checkState() == Qt.Checked - - if checked: - self.add_camera(topic) + msg_type = item.data(Qt.UserRole) + + if item.checkState() == Qt.Checked: + self.add_camera(topic, msg_type) else: self.remove_camera(topic) - # -------------------- - - def add_camera(self, topic): - if topic in self.camera_widgets: - return - + def add_camera(self, topic, msg_type): + if topic in self.camera_widgets: return + widget = CameraWidget(topic) - signal = ImageSignal() - signal.image_ready.connect(widget.update_image) - - self.ros_node.image_signals[topic] = signal - self.ros_node.subscribe(topic) - + sig = ImageSignal() + sig.image_ready.connect(widget.update_image) + self.ros_node.image_signals[topic] = sig + + self.ros_node.subscribe(topic, msg_type) self.camera_widgets[topic] = widget self.rebuild_grid() def remove_camera(self, topic): - if topic not in self.camera_widgets: - return - - self.ros_node.unsubscribe(topic) - - widget = self.camera_widgets[topic] - widget.setParent(None) - widget.deleteLater() - - del self.camera_widgets[topic] - self.rebuild_grid() - + if topic in self.camera_widgets: + self.ros_node.unsubscribe(topic) + if topic in self.ros_node.image_signals: + del self.ros_node.image_signals[topic] + + widget = self.camera_widgets.pop(topic) + self.grid_layout.removeWidget(widget) + widget.deleteLater() + self.rebuild_grid() - # -------------------- - def rebuild_grid(self): - # Clear previous stretches to prevent "dead space" - for i in range(self.grid_layout.rowCount()): - self.grid_layout.setRowStretch(i, 0) - for i in range(self.grid_layout.columnCount()): - self.grid_layout.setColumnStretch(i, 0) - - # Properly clear the layout - while self.grid_layout.count(): - item = self.grid_layout.takeAt(0) + for i in reversed(range(self.grid_layout.count())): + item = self.grid_layout.itemAt(i) if item.widget(): - item.widget().hide() - - widgets = list(self.camera_widgets.values()) - count = len(widgets) + item.widget().setParent(None) - if count == 0: - return + widgets = list(self.camera_widgets.values()) + if not widgets: return + count = len(widgets) cols = math.ceil(math.sqrt(count)) - rows = math.ceil(count / cols) - - index = 0 - for r in range(rows): - for c in range(cols): - if index >= count: - break - - widget = widgets[index] - self.grid_layout.addWidget(widget, r, c) - - # Ensure it's visible and active - widget.setVisible(True) - widget.update() # Triggers a repaint of the camera frame - - index += 1 - - for r in range(rows): - self.grid_layout.setRowStretch(r, 1) - for c in range(cols): - self.grid_layout.setColumnStretch(c, 1) - - self.grid_layout.setContentsMargins(0, 0, 0, 0) - self.grid_layout.setSpacing(1) + for i, widget in enumerate(widgets): + row, col = divmod(i, cols) + self.grid_layout.addWidget(widget, row, col) - -# -------------------- Main -------------------- +# -------------------- Execution -------------------- def main(): rclpy.init() - + + node = UserInterfaceNode() + ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + ros_thread.start() + app = QApplication(sys.argv) app.setStyleSheet(dark_stylesheet) - - ros_node = UserInterfaceNode() - - ros_thread = threading.Thread( - target=rclpy.spin, - args=(ros_node,), - daemon=True - ) - ros_thread.start() - - win = MultiCameraWindow(ros_node) - # win.resize(1200, 800) - win.show() - - sys.exit(app.exec()) + + gui = MultiCameraWindow(node) + gui.resize(1280, 720) + gui.show() + + try: + # FIX 2: Do not wrap in sys.exit() yet to allow clean shutdown block + app.exec() + finally: + # FIX 3: Check if rclpy is still okay before shutting down + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/src/camera_interface/camera_interface/webcam.py b/src/camera_interface/camera_interface/webcam.py index 7b259749..7f4ec84c 100644 --- a/src/camera_interface/camera_interface/webcam.py +++ b/src/camera_interface/camera_interface/webcam.py @@ -1,54 +1,147 @@ #!/usr/bin/env python3 - import rclpy from rclpy.node import Node -from sensor_msgs.msg import Image - +from sensor_msgs.msg import CompressedImage import cv2 -from cv_bridge import CvBridge +import subprocess +import threading +import time -class WebcamPublisher(Node): +class GpuWebcamPublisher(Node): def __init__(self): - super().__init__('webcam_node') - self.publisher_ = self.create_publisher(Image, '/webcam/image_raw', 10) - self.publisher1_ = self.create_publisher(Image, '/webcam/image_raw_1', 10) - self.publisher2_ = self.create_publisher(Image, '/webcam/image_raw_2', 10) - self.publisher3_ = self.create_publisher(Image, '/webcam/image_raw_3', 10) - self.timer = self.create_timer(0.01, self.timer_callback) - self.bridge = CvBridge() - self.cap = cv2.VideoCapture(0) - - def timer_callback(self): - msg = Image() + super().__init__('webcam_gpu_node') + + # Configuration + self.device_id = 0 + self.fps = 30 + + # 1. Setup Camera with V4L2 Backend + # 'cv2.CAP_V4L2' prevents the GStreamer "Internal data stream error" + self.cap = cv2.VideoCapture(self.device_id, cv2.CAP_V4L2) + + # CRITICAL: Force MJPEG. + # Most USB cams cannot do 1280x720 @ 30fps in raw YUYV format (bandwidth limit). + self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G')) + + # Force Resolution + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + self.cap.set(cv2.CAP_PROP_FPS, self.fps) + + # Read actual parameters (in case camera rejected our request) + w = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 640 + h = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 480 + + # NVENC requires even dimensions + self.width = int(w) if int(w) % 2 == 0 else int(w) - 1 + self.height = int(h) if int(h) % 2 == 0 else int(h) - 1 + + # Codec Settings + self.codec = 'hevc_nvenc' # NVIDIA Hardware Encoder + self.stream_fmt = 'hevc' # Raw HEVC stream + + # 2. Setup FFmpeg + self.ffmpeg_cmd = [ + 'ffmpeg', '-y', + '-hide_banner', '-loglevel', 'error', + '-f', 'rawvideo', + '-vcodec', 'rawvideo', + '-s', f'{self.width}x{self.height}', + '-pix_fmt', 'bgr24', + '-r', str(self.fps), + '-i', '-', # Input from pipe + '-c:v', self.codec, + '-preset', 'p1', # Low latency preset + '-tune', 'ull', # Ultra Low Latency + '-zerolatency', '1', + '-g', '30', # Keyframe every 1s + '-bf', '0', # No B-frames + '-f', self.stream_fmt, + '-' # Output to pipe + ] - if not self.cap.isOpened(): - self.get_logger().error('Error: Could not open webcam.') - self.destroy_node() + try: + self.process = subprocess.Popen( + self.ffmpeg_cmd, + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + bufsize=0 # Unbuffered IO + ) + except FileNotFoundError: + self.get_logger().error("FFmpeg not found. Ensure ffmpeg is installed.") + return + + self.publisher_ = self.create_publisher(CompressedImage, '/webcam/gpu_stream', 10) + + # 3. Start Threads + self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) - success, frame = self.cap.read() + self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) + self.output_thread.start() + + self.get_logger().info(f"Streaming {self.width}x{self.height} via {self.codec} (MJPG Input)...") - # If frame is not read successfully, break the loop - if not success: - self.get_logger().warn("Error: Could not read frame.") + def feed_encoder(self): + """Capture frame and push to FFmpeg stdin""" + if not self.cap.isOpened(): return + + ret, frame = self.cap.read() + if ret: + # Resize if the camera ignores our resolution request + if frame.shape[1] != self.width or frame.shape[0] != self.height: + frame = cv2.resize(frame, (self.width, self.height)) - msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') + try: + self.process.stdin.write(frame.tobytes()) + self.process.stdin.flush() + except BrokenPipeError: + self.get_logger().error("FFmpeg stdin broken pipe") + rclpy.shutdown() + else: + self.get_logger().warn("Camera frame read failed.") - self.get_logger().info('Publishing: laptop webcam frame') + def read_encoded_stream(self): + """Continuously read from FFmpeg stdout and publish""" + # Buffer size large enough for 1080p frames to prevent artifacting + chunk_size = 256 * 1024 - self.publisher_.publish(msg) - self.publisher1_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, 0), encoding='bgr8')) - self.publisher2_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, 1), encoding='bgr8')) - self.publisher3_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, -1), encoding='bgr8')) + while rclpy.ok() and self.process.poll() is None: + try: + # Read whatever is available + data = self.process.stdout.read(chunk_size) + + if data: + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.format = self.stream_fmt + msg.data = data + + self.publisher_.publish(msg) + else: + time.sleep(0.002) + except Exception as e: + self.get_logger().error(f"Read error: {e}") + break + + def destroy_node(self): + if self.process: + self.process.kill() + self.cap.release() + super().destroy_node() def main(args=None): rclpy.init(args=args) - webcam_publisher = WebcamPublisher() - - rclpy.spin(webcam_publisher) - webcam_publisher.destroy_node() - - rclpy.shutdown() + node = GpuWebcamPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() if __name__ == '__main__': main() \ No newline at end of file From 3b1ba06e4eefa033494c0424bbb0a27dd436e61f Mon Sep 17 00:00:00 2001 From: alex berg Date: Tue, 24 Feb 2026 19:01:18 -0600 Subject: [PATCH 04/21] fix broken deepstream file --- docker/Dockerfile.deepstream | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docker/Dockerfile.deepstream b/docker/Dockerfile.deepstream index 44ac7782..8c10570c 100644 --- a/docker/Dockerfile.deepstream +++ b/docker/Dockerfile.deepstream @@ -5,7 +5,9 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o COPY deepstream/deepstream.deb deepstream.deb -RUN --mount=type=cache,target=/var/cache/apt apt-get update && apt-get install -y \ +RUN --mount=type=cache,target=/var/cache/apt \ + apt-get update --allow-releaseinfo-change && \ + apt-get install -y --no-install-recommends \ gstreamer1.0-tools \ gstreamer1.0-plugins-base \ gstreamer1.0-plugins-good \ From f97553e3021c328ce439031ab5890681b19c3c80 Mon Sep 17 00:00:00 2001 From: alex berg Date: Tue, 24 Feb 2026 19:06:43 -0600 Subject: [PATCH 05/21] readme update --- docker/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docker/README.md b/docker/README.md index 6307fa40..41633b54 100644 --- a/docker/README.md +++ b/docker/README.md @@ -9,6 +9,9 @@ Find the highest level image of this build with docker image list. The highest l Tag this image as `umnrobotics/:_` At time of writing, we were tagging images as 'umnrobotics/isaac_ros3.1:x86_64-ros2_humble-realsense-deepstream-user-zed-umn-gazebo_d000f8df5f3859fd56c7459b2ad3a718' +to find the hash, run the build command again, and find the first line starting with "Checking if base image ... exists on remote repository. +The image ID listed here is the one you should use. It is generated based on the dockerfiles in mysterious ways, so just copy paste it from the terminal log. + If you are logged in to docker, running docker image push will push this image to the cloud, and you are done. If you changed the name of/created a new dockerhub repo, update BASE_DOCKER_REGISTRY_NAMES in /scripts/build_image.sh to reflect the new name. From d464a6a75f50e0bcd928fc7a38c8eadea81726e4 Mon Sep 17 00:00:00 2001 From: Adam Yaj Date: Thu, 26 Feb 2026 19:03:14 -0600 Subject: [PATCH 06/21] Implement general compression launch file for testing --- .../camera_interface/compression.py | 189 +++++++++ .../camera_interface/qt_user_interface.py | 395 +++++++++--------- .../camera_interface/webcam_raw.py | 54 +++ .../launch/camera_system_launch.py | 30 ++ src/camera_interface/setup.py | 8 +- 5 files changed, 485 insertions(+), 191 deletions(-) create mode 100644 src/camera_interface/camera_interface/compression.py create mode 100644 src/camera_interface/camera_interface/webcam_raw.py create mode 100644 src/camera_interface/launch/camera_system_launch.py diff --git a/src/camera_interface/camera_interface/compression.py b/src/camera_interface/camera_interface/compression.py new file mode 100644 index 00000000..dd51bd57 --- /dev/null +++ b/src/camera_interface/camera_interface/compression.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import ParameterDescriptor, ParameterType +from sensor_msgs.msg import CompressedImage +import cv2 +import subprocess +import threading +import time + +class VideoCompressionNode(Node): + def __init__(self): + super().__init__('video_compression_node') + + self.initalize_parameters() + + # Get the parameter value + self.topic_name = self.get_parameter('topic_name').get_parameter_value().string_value + self.device_id = self.get_parameter('device_id').get_parameter_value().string_value + + # Conversion Logic + try: + # If the string is just a number (e.g., "0"), convert to int + if str(self.device_id).isdigit(): + self.camera_source = int(self.device_id) + else: + # If it's a path (e.g., "/dev/video0"), keep it as a string + self.camera_source = str(self.device_id) + except Exception as e: + self.get_logger().error(f"Invalid device_id: {e}") + + self.get_logger().info(f"Capturing camera_source: {self.camera_source}") + + self.fps = 30 + + # 1. Setup Camera with V4L2 Backend + # 'cv2.CAP_V4L2' prevents the GStreamer "Internal data stream error" + self.cap = cv2.VideoCapture(self.camera_source, cv2.CAP_V4L2) + + # CRITICAL: Force MJPEG. + # Most USB cams cannot do 1280x720 @ 30fps in raw YUYV format (bandwidth limit). + self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G')) + + # Force Resolution + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + self.cap.set(cv2.CAP_PROP_FPS, self.fps) + + # Read actual parameters (in case camera rejected our request) + w = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 640 + h = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 480 + + # NVENC requires even dimensions + self.width = int(w) if int(w) % 2 == 0 else int(w) - 1 + self.height = int(h) if int(h) % 2 == 0 else int(h) - 1 + + # Codec Settings + self.codec = 'hevc_nvenc' # NVIDIA Hardware Encoder + self.stream_fmt = 'hevc' # Raw HEVC stream + + # 2. Setup FFmpeg + self.ffmpeg_cmd = [ + 'ffmpeg', '-y', + '-hide_banner', '-loglevel', 'error', + '-f', 'rawvideo', + '-vcodec', 'rawvideo', + '-s', f'{self.width}x{self.height}', + '-pix_fmt', 'bgr24', + '-r', str(self.fps), + '-i', '-', # Input from pipe + '-c:v', self.codec, + '-preset', 'p1', # Low latency preset + '-tune', 'ull', # Ultra Low Latency + '-zerolatency', '1', + '-g', '30', # Keyframe every 1s + '-bf', '0', # No B-frames + '-f', self.stream_fmt, + '-' # Output to pipe + ] + + try: + self.process = subprocess.Popen( + self.ffmpeg_cmd, + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + bufsize=0 # Unbuffered IO + ) + except FileNotFoundError: + self.get_logger().error("FFmpeg not found. Ensure ffmpeg is installed.") + return + + self.publisher_ = self.create_publisher(CompressedImage, self.topic_name, 10) + + # 3. Start Threads + self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) + + self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) + self.output_thread.start() + + self.get_logger().info(f"Streaming {self.width}x{self.height} via {self.codec} (MJPG Input)...") + + def initalize_parameters(self): + # Define the parameter descriptor to specify the type + topic_parameter_descriptor = ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description='A wecam topic to publish to.' + ) + + camera_id_parameter_descriptor = ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description='A wecam id to read from.' + ) + + self.declare_parameter( + 'topic_name', + value='', + descriptor=topic_parameter_descriptor + ) + + self.declare_parameter( + 'device_id', + value='0', + descriptor=camera_id_parameter_descriptor + ) + + def feed_encoder(self): + """Capture frame and push to FFmpeg stdin""" + if not self.cap.isOpened(): + return + + ret, frame = self.cap.read() + if ret: + # Resize if the camera ignores our resolution request + if frame.shape[1] != self.width or frame.shape[0] != self.height: + frame = cv2.resize(frame, (self.width, self.height)) + + try: + self.process.stdin.write(frame.tobytes()) + self.process.stdin.flush() + except BrokenPipeError: + self.get_logger().error("FFmpeg stdin broken pipe") + rclpy.shutdown() + else: + self.get_logger().warn("Camera frame read failed.") + + def read_encoded_stream(self): + """Continuously read from FFmpeg stdout and publish""" + # Buffer size large enough for 1080p frames to prevent artifacting + chunk_size = 256 * 1024 + + while rclpy.ok() and self.process.poll() is None: + try: + # Read whatever is available + data = self.process.stdout.read(chunk_size) + + if data: + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.format = self.stream_fmt + msg.data = data + + self.publisher_.publish(msg) + else: + time.sleep(0.002) + except Exception as e: + self.get_logger().error(f"Read error: {e}") + break + + def destroy_node(self): + if self.process: + self.process.kill() + self.cap.release() + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = VideoCompressionNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/camera_interface/camera_interface/qt_user_interface.py b/src/camera_interface/camera_interface/qt_user_interface.py index d224148d..10cfb9fe 100644 --- a/src/camera_interface/camera_interface/qt_user_interface.py +++ b/src/camera_interface/camera_interface/qt_user_interface.py @@ -2,20 +2,19 @@ import threading import time import math -import numpy as np -import av # PyAV import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy -from sensor_msgs.msg import Image, CompressedImage +from sensor_msgs.msg import Image from cv_bridge import CvBridge +import cv2 from PySide6.QtWidgets import ( QApplication, QMainWindow, QLabel, QPushButton, QVBoxLayout, QHBoxLayout, QWidget, QListWidget, QListWidgetItem, - QGridLayout, QSizePolicy # <--- Added QSizePolicy import + QGridLayout, QSizePolicy ) from PySide6.QtGui import QImage, QPixmap from PySide6.QtCore import Qt, Signal, QObject @@ -23,54 +22,38 @@ # -------------------- Dark Theme -------------------- dark_stylesheet = """ -QWidget { background-color: #121212; color: #e0e0e0; } -QPushButton { background-color: #1f1f1f; border: 1px solid #333; padding: 6px; } -QPushButton:hover { background-color: #333; } -QListWidget { background-color: #1c1c1c; border: 1px solid #333; } +QWidget { + background-color: #121212; + color: #e0e0e0; +} +QPushButton { + background-color: #1f1f1f; + border: 1px solid #333; + padding: 6px; +} +QListWidget { + background-color: #1c1c1c; +} """ -# -------------------- Decoding Logic -------------------- +# -------------------- Qt Signal -------------------- class ImageSignal(QObject): image_ready = Signal(object) -class StreamDecoder: - """Stateful decoder using PyAV for AV1/HEVC/H264 streams.""" - def __init__(self, codec_name='av1'): - try: - self.codec = av.CodecContext.create(codec_name, 'r') - except Exception as e: - print(f"Error creating codec {codec_name}: {e}") - self.codec = None - - def decode(self, binary_data): - if not self.codec: - return None - - try: - packets = self.codec.parse(binary_data) - for packet in packets: - frames = self.codec.decode(packet) - for frame in frames: - return frame.to_ndarray(format='rgb24') - except Exception as e: - print(f"Decode error: {e}") - return None - # -------------------- ROS Node -------------------- class UserInterfaceNode(Node): def __init__(self): super().__init__('multi_camera_qt_interface') self.bridge = CvBridge() - self.subscriptions_map = {} + self.camera_subscriptions = {} self.image_signals = {} - self.decoders = {} - self.last_frame_time = {} - self.max_fps = 60.0 + self.last_frame = {} + self.max_fps = 30.0 - def subscribe(self, topic, msg_type_str): - if topic in self.subscriptions_map: + def subscribe(self, topic): + if topic in self.camera_subscriptions: return qos = QoSProfile( @@ -78,235 +61,269 @@ def subscribe(self, topic, msg_type_str): history=QoSHistoryPolicy.KEEP_LAST, depth=1 ) - self.last_frame_time[topic] = 0.0 - # Map string type to class type - if 'CompressedImage' in str(msg_type_str): - msg_type = CompressedImage - else: - msg_type = Image + self.last_frame[topic] = 0.0 - self.subscriptions_map[topic] = self.create_subscription( - msg_type, + self.camera_subscriptions[topic] = self.create_subscription( + Image, topic, lambda msg, t=topic: self.callback(msg, t), qos ) def unsubscribe(self, topic): - if topic in self.subscriptions_map: - self.destroy_subscription(self.subscriptions_map[topic]) - del self.subscriptions_map[topic] - del self.last_frame_time[topic] - if topic in self.decoders: - del self.decoders[topic] + if topic in self.camera_subscriptions: + self.destroy_subscription(self.camera_subscriptions[topic]) + del self.camera_subscriptions[topic] + del self.last_frame[topic] + del self.image_signals[topic] def callback(self, msg, topic): + # Topic removed while callback was queued if topic not in self.image_signals: return now = time.time() - if now - self.last_frame_time.get(topic, 0.0) < 1.0 / self.max_fps: + if now - self.last_frame.get(topic, 0.0) < 1.0 / self.max_fps: return - self.last_frame_time[topic] = now - - cv_img = None - try: - if isinstance(msg, CompressedImage): - if topic not in self.decoders: - fmt = msg.format.lower() - codec = 'hevc' if 'hevc' in fmt else 'av1' if 'av1' in fmt else 'h264' - self.decoders[topic] = StreamDecoder(codec) - - cv_img = self.decoders[topic].decode(bytes(msg.data)) - - elif isinstance(msg, Image): - if "av1" in msg.encoding.lower(): - if topic not in self.decoders: - self.decoders[topic] = StreamDecoder('av1') - cv_img = self.decoders[topic].decode(bytes(msg.data)) - else: - cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') - - if cv_img is not None: - signal = self.image_signals.get(topic) - if signal: - signal.image_ready.emit(cv_img) - - except Exception as e: - self.get_logger().error(f"Processing error on {topic}: {e}") - -# -------------------- UI Components -------------------- + + self.last_frame[topic] = now + + cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') + + signal = self.image_signals.get(topic) + if signal is not None: + signal.image_ready.emit(cv_img) + +# -------------------- Camera Widget -------------------- class CameraWidget(QWidget): def __init__(self, topic): super().__init__() self.topic = topic - self.last_frame = None - self.label = QLabel("Waiting for Stream...") + self.label = QLabel("Waiting for image...") self.label.setAlignment(Qt.AlignCenter) - self.label.setStyleSheet("background-color: #000; color: #555;") - - # FIX 1: Use QSizePolicy instead of QWidget.SizePolicy - self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) - + self.label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Ignored) + self.label.setMinimumSize(1, 1) + title = QLabel(topic) title.setAlignment(Qt.AlignCenter) - title.setStyleSheet("font-size: 10px; color: #888; background: #1a1a1a; padding: 2px;") - - layout = QVBoxLayout(self) + title.setFixedHeight(18) + title.setStyleSheet("font-size:10px; border-top:1px solid #333") + + # ---- Image container ---- + image_container = QWidget() + image_layout = QVBoxLayout() + image_layout.setContentsMargins(0, 0, 0, 0) + image_layout.addWidget(self.label) + # image_layout.addWidget(title, 0) + image_container.setLayout(image_layout) + + image_container.setSizePolicy( + QSizePolicy.Expanding, + QSizePolicy.Ignored + ) + + # ---- Main layout ---- + layout = QVBoxLayout() layout.setContentsMargins(0, 0, 0, 0) layout.setSpacing(0) - layout.addWidget(self.label, 1) - layout.addWidget(title) + + layout.addWidget(title, 1) # stays attached + layout.addWidget(image_container, 0) # stretches + + self.setLayout(layout) def update_image(self, cv_img): - self.last_frame = cv_img + self.last_frame = cv_img # cache frame + h, w, ch = cv_img.shape - bytes_per_line = ch * w - - qimg = QImage(cv_img.data, w, h, bytes_per_line, QImage.Format_RGB888).copy() - + qimg = QImage(cv_img.data, w, h, ch * w, QImage.Format_RGB888) pix = QPixmap.fromImage(qimg) - self.label.setPixmap(pix.scaled( - self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation - )) + + self.label.setPixmap( + pix.scaled( + self.label.size(), + Qt.KeepAspectRatio, + Qt.SmoothTransformation + ) + ) def resizeEvent(self, event): - if self.last_frame is not None: + if hasattr(self, "last_frame"): self.update_image(self.last_frame) - super().resizeEvent(event) + +# -------------------- Main Window -------------------- class MultiCameraWindow(QMainWindow): def __init__(self, ros_node): super().__init__() + self.setWindowTitle("ROS2 Multi-Camera Viewer") self.ros_node = ros_node + self.camera_widgets = {} - self.setWindowTitle("ROS2 Stream Viewer") main_widget = QWidget() - self.layout = QHBoxLayout(main_widget) - - side_panel = QWidget() - side_layout = QVBoxLayout(side_panel) + main_layout = QHBoxLayout() + + # ---- Sidebar ---- + side_layout = QVBoxLayout() + self.refresh_btn = QPushButton("Refresh Topics") + self.refresh_btn.clicked.connect(self.refresh_topics) + self.topic_list = QListWidget() self.topic_list.itemChanged.connect(self.topic_toggled) - - btn_refresh = QPushButton("Refresh Topics") - btn_refresh.clicked.connect(self.refresh_topics) - - side_layout.addWidget(QLabel("Available Topics:")) + side_layout.addWidget(self.topic_list) - side_layout.addWidget(btn_refresh) - + side_layout.addWidget(self.refresh_btn) + + # ---- Grid Area ---- self.grid_widget = QWidget() - self.grid_layout = QGridLayout(self.grid_widget) - self.grid_layout.setContentsMargins(0,0,0,0) - - self.layout.addWidget(side_panel, 1) - self.layout.addWidget(self.grid_widget, 4) + self.grid_layout = QGridLayout() + self.grid_widget.setLayout(self.grid_layout) + self.grid_widget.setSizePolicy( + QSizePolicy.Expanding, + QSizePolicy.Expanding + ) + + + main_layout.addLayout(side_layout) + main_layout.addWidget(self.grid_widget, 1) + + main_widget.setLayout(main_layout) self.setCentralWidget(main_widget) - + self.refresh_topics() + # -------------------- + def refresh_topics(self): self.topic_list.blockSignals(True) - checked = {self.topic_list.item(i).text() for i in range(self.topic_list.count()) - if self.topic_list.item(i).checkState() == Qt.Checked} - self.topic_list.clear() - - try: - topic_names_and_types = self.ros_node.get_topic_names_and_types() - except Exception as e: - print(f"Error fetching topics: {e}") - topic_names_and_types = [] - - for name, types in topic_names_and_types: - # Check for relevant message types - if any(t in ['sensor_msgs/msg/Image', 'sensor_msgs/msg/CompressedImage'] for t in types): + + topics = self.ros_node.get_topic_names_and_types() + for name, types in topics: + if "sensor_msgs/msg/Image" in types: item = QListWidgetItem(name) item.setFlags(item.flags() | Qt.ItemIsUserCheckable) - item.setCheckState(Qt.Checked if name in checked else Qt.Unchecked) - # Store the type string for later use - item.setData(Qt.UserRole, types[0]) + item.setCheckState(Qt.Unchecked) self.topic_list.addItem(item) - + self.topic_list.blockSignals(False) + # -------------------- + def topic_toggled(self, item): topic = item.text() - msg_type = item.data(Qt.UserRole) - - if item.checkState() == Qt.Checked: - self.add_camera(topic, msg_type) + checked = item.checkState() == Qt.Checked + + if checked: + self.add_camera(topic) else: self.remove_camera(topic) - def add_camera(self, topic, msg_type): - if topic in self.camera_widgets: return - + # -------------------- + + def add_camera(self, topic): + if topic in self.camera_widgets: + return + widget = CameraWidget(topic) - sig = ImageSignal() - sig.image_ready.connect(widget.update_image) - self.ros_node.image_signals[topic] = sig - - self.ros_node.subscribe(topic, msg_type) + signal = ImageSignal() + signal.image_ready.connect(widget.update_image) + + self.ros_node.image_signals[topic] = signal + self.ros_node.subscribe(topic) + self.camera_widgets[topic] = widget self.rebuild_grid() def remove_camera(self, topic): - if topic in self.camera_widgets: - self.ros_node.unsubscribe(topic) - if topic in self.ros_node.image_signals: - del self.ros_node.image_signals[topic] - - widget = self.camera_widgets.pop(topic) - self.grid_layout.removeWidget(widget) - widget.deleteLater() - self.rebuild_grid() + if topic not in self.camera_widgets: + return + self.ros_node.unsubscribe(topic) + + widget = self.camera_widgets[topic] + widget.setParent(None) + widget.deleteLater() + + del self.camera_widgets[topic] + self.rebuild_grid() + + + # -------------------- + def rebuild_grid(self): - for i in reversed(range(self.grid_layout.count())): - item = self.grid_layout.itemAt(i) + # Clear previous stretches to prevent "dead space" + for i in range(self.grid_layout.rowCount()): + self.grid_layout.setRowStretch(i, 0) + for i in range(self.grid_layout.columnCount()): + self.grid_layout.setColumnStretch(i, 0) + + # Properly clear the layout + while self.grid_layout.count(): + item = self.grid_layout.takeAt(0) if item.widget(): - item.widget().setParent(None) - + item.widget().hide() + widgets = list(self.camera_widgets.values()) - if not widgets: return - count = len(widgets) + + if count == 0: + return + cols = math.ceil(math.sqrt(count)) + rows = math.ceil(count / cols) + + index = 0 + for r in range(rows): + for c in range(cols): + if index >= count: + break + + widget = widgets[index] + self.grid_layout.addWidget(widget, r, c) + + # Ensure it's visible and active + widget.setVisible(True) + widget.update() # Triggers a repaint of the camera frame + + index += 1 + + for r in range(rows): + self.grid_layout.setRowStretch(r, 1) + for c in range(cols): + self.grid_layout.setColumnStretch(c, 1) + + self.grid_layout.setContentsMargins(0, 0, 0, 0) + self.grid_layout.setSpacing(1) - for i, widget in enumerate(widgets): - row, col = divmod(i, cols) - self.grid_layout.addWidget(widget, row, col) -# -------------------- Execution -------------------- + +# -------------------- Main -------------------- def main(): rclpy.init() - - node = UserInterfaceNode() - ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) - ros_thread.start() - + app = QApplication(sys.argv) app.setStyleSheet(dark_stylesheet) - - gui = MultiCameraWindow(node) - gui.resize(1280, 720) - gui.show() - - try: - # FIX 2: Do not wrap in sys.exit() yet to allow clean shutdown block - app.exec() - finally: - # FIX 3: Check if rclpy is still okay before shutting down - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() + + ros_node = UserInterfaceNode() + + ros_thread = threading.Thread( + target=rclpy.spin, + args=(ros_node,), + daemon=True + ) + ros_thread.start() + + win = MultiCameraWindow(ros_node) + # win.resize(1200, 800) + win.show() + + sys.exit(app.exec()) if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/src/camera_interface/camera_interface/webcam_raw.py b/src/camera_interface/camera_interface/webcam_raw.py new file mode 100644 index 00000000..ec0b3a10 --- /dev/null +++ b/src/camera_interface/camera_interface/webcam_raw.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image + +import cv2 +from cv_bridge import CvBridge + +class WebcamPublisher(Node): + def __init__(self): + super().__init__('webcam_node_raw') + self.publisher_ = self.create_publisher(Image, '/webcam/image_raw', 10) + self.publisher1_ = self.create_publisher(Image, '/webcam/image_raw_1', 10) + self.publisher2_ = self.create_publisher(Image, '/webcam/image_raw_2', 10) + self.publisher3_ = self.create_publisher(Image, '/webcam/image_raw_3', 10) + self.timer = self.create_timer(0.01, self.timer_callback) + self.bridge = CvBridge() + self.cap = cv2.VideoCapture(0) + + def timer_callback(self): + msg = Image() + + if not self.cap.isOpened(): + self.get_logger().error('Error: Could not open webcam.') + self.destroy_node() + + success, frame = self.cap.read() + + # If frame is not read successfully, break the loop + if not success: + self.get_logger().warn("Error: Could not read frame.") + return + + msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8') + + self.get_logger().info('Publishing: laptop webcam frame') + + self.publisher_.publish(msg) + self.publisher1_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, 0), encoding='bgr8')) + self.publisher2_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, 1), encoding='bgr8')) + self.publisher3_.publish(self.bridge.cv2_to_imgmsg(cv2.flip(frame, -1), encoding='bgr8')) + +def main(args=None): + rclpy.init(args=args) + webcam_publisher = WebcamPublisher() + + rclpy.spin(webcam_publisher) + webcam_publisher.destroy_node() + + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/camera_interface/launch/camera_system_launch.py b/src/camera_interface/launch/camera_system_launch.py new file mode 100644 index 00000000..42b07522 --- /dev/null +++ b/src/camera_interface/launch/camera_system_launch.py @@ -0,0 +1,30 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + # NOTE: Instead of an index, you can use the stable path found in /dev/v4l/by-id/ or /dev/v4l/by-path/ + # (e.g.) -> /dev/v4l/by-id/usb-Logitech_Webcam_C920_ABC123-video-index0 + camera_configs = [ + {"name": "right", "topic": "/webcam/right", "id": "/dev/video6"}, + {"name": "left", "topic": "/webcam/left", "id": "/dev/video8"}, + {"name": "back", "topic": "/webcam/back", "id": "/dev/video0"}, + {"name": "digger", "topic": "/webcam/digger", "id": "/dev/video4"}, + {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/video5"}, + {"name": "front", "topic": "/webcam/front", "id": "/dev/video0"}, + ] + + nodes = [ + Node( + package='camera_interface', + executable='compression', + name=f"compressor_{config['name']}", + parameters=[{ + 'topic_name': config['topic'], + 'device_id': config['id'] + }], + output='screen' + ) + for config in camera_configs + ] + + return LaunchDescription(nodes) \ No newline at end of file diff --git a/src/camera_interface/setup.py b/src/camera_interface/setup.py index e1a59aaa..ae92bfe3 100644 --- a/src/camera_interface/setup.py +++ b/src/camera_interface/setup.py @@ -9,7 +9,9 @@ data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']) + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', + ['launch/camera_system_launch.py']), ], install_requires=['setuptools'], zip_safe=True, @@ -25,7 +27,9 @@ entry_points={ 'console_scripts': [ 'qt_user_interface = camera_interface.qt_user_interface:main', - 'webcam = camera_interface.webcam:main' + 'webcam = camera_interface.webcam:main', + 'webcam_raw = camera_interface.webcam_raw:main', + 'compression = camera_interface.compression:main' ], }, ) From aa9780fd8b8c072f6341d18a6b4374b63fd3ce72 Mon Sep 17 00:00:00 2001 From: Adam Yaj Date: Tue, 3 Mar 2026 18:32:58 -0600 Subject: [PATCH 07/21] update logging to include topic output on startup --- src/camera_interface/camera_interface/compression.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/camera_interface/camera_interface/compression.py b/src/camera_interface/camera_interface/compression.py index dd51bd57..a22d49e4 100644 --- a/src/camera_interface/camera_interface/compression.py +++ b/src/camera_interface/camera_interface/compression.py @@ -29,7 +29,7 @@ def __init__(self): except Exception as e: self.get_logger().error(f"Invalid device_id: {e}") - self.get_logger().info(f"Capturing camera_source: {self.camera_source}") + self.get_logger().info(f"Capturing \"{self.camera_source}\" : \"{self.topic_name}\"") self.fps = 30 From 16da803d15fc6a91d12a52b3978c849ba9934a42 Mon Sep 17 00:00:00 2001 From: Adam Yaj Date: Tue, 3 Mar 2026 19:25:11 -0600 Subject: [PATCH 08/21] Revert overwirte of compression.py file --- .../camera_interface/compression.py | 448 +++++++++++------- 1 file changed, 286 insertions(+), 162 deletions(-) diff --git a/src/camera_interface/camera_interface/compression.py b/src/camera_interface/camera_interface/compression.py index a22d49e4..9d107e48 100644 --- a/src/camera_interface/camera_interface/compression.py +++ b/src/camera_interface/camera_interface/compression.py @@ -1,189 +1,313 @@ #!/usr/bin/env python3 -import rclpy -from rclpy.node import Node -from rcl_interfaces.msg import ParameterDescriptor, ParameterType -from sensor_msgs.msg import CompressedImage -import cv2 -import subprocess +import sys import threading import time +import math +import numpy as np +import av # PyAV -class VideoCompressionNode(Node): - def __init__(self): - super().__init__('video_compression_node') - - self.initalize_parameters() - - # Get the parameter value - self.topic_name = self.get_parameter('topic_name').get_parameter_value().string_value - self.device_id = self.get_parameter('device_id').get_parameter_value().string_value - - # Conversion Logic +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +from sensor_msgs.msg import Image, CompressedImage +from cv_bridge import CvBridge + +from PySide6.QtWidgets import ( + QApplication, QMainWindow, QLabel, QPushButton, + QVBoxLayout, QHBoxLayout, QWidget, + QListWidget, QListWidgetItem, + QGridLayout, QSizePolicy # <--- Added QSizePolicy import +) +from PySide6.QtGui import QImage, QPixmap +from PySide6.QtCore import Qt, Signal, QObject + +# -------------------- Dark Theme -------------------- + +dark_stylesheet = """ +QWidget { background-color: #121212; color: #e0e0e0; } +QPushButton { background-color: #1f1f1f; border: 1px solid #333; padding: 6px; } +QPushButton:hover { background-color: #333; } +QListWidget { background-color: #1c1c1c; border: 1px solid #333; } +""" + +# -------------------- Decoding Logic -------------------- + +class ImageSignal(QObject): + image_ready = Signal(object) + +class StreamDecoder: + """Stateful decoder using PyAV for AV1/HEVC/H264 streams.""" + def __init__(self, codec_name='av1'): try: - # If the string is just a number (e.g., "0"), convert to int - if str(self.device_id).isdigit(): - self.camera_source = int(self.device_id) - else: - # If it's a path (e.g., "/dev/video0"), keep it as a string - self.camera_source = str(self.device_id) + self.codec = av.CodecContext.create(codec_name, 'r') except Exception as e: - self.get_logger().error(f"Invalid device_id: {e}") - - self.get_logger().info(f"Capturing \"{self.camera_source}\" : \"{self.topic_name}\"") - - self.fps = 30 - - # 1. Setup Camera with V4L2 Backend - # 'cv2.CAP_V4L2' prevents the GStreamer "Internal data stream error" - self.cap = cv2.VideoCapture(self.camera_source, cv2.CAP_V4L2) - - # CRITICAL: Force MJPEG. - # Most USB cams cannot do 1280x720 @ 30fps in raw YUYV format (bandwidth limit). - self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G')) - - # Force Resolution - self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) - self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) - self.cap.set(cv2.CAP_PROP_FPS, self.fps) - - # Read actual parameters (in case camera rejected our request) - w = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 640 - h = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 480 - - # NVENC requires even dimensions - self.width = int(w) if int(w) % 2 == 0 else int(w) - 1 - self.height = int(h) if int(h) % 2 == 0 else int(h) - 1 - - # Codec Settings - self.codec = 'hevc_nvenc' # NVIDIA Hardware Encoder - self.stream_fmt = 'hevc' # Raw HEVC stream - - # 2. Setup FFmpeg - self.ffmpeg_cmd = [ - 'ffmpeg', '-y', - '-hide_banner', '-loglevel', 'error', - '-f', 'rawvideo', - '-vcodec', 'rawvideo', - '-s', f'{self.width}x{self.height}', - '-pix_fmt', 'bgr24', - '-r', str(self.fps), - '-i', '-', # Input from pipe - '-c:v', self.codec, - '-preset', 'p1', # Low latency preset - '-tune', 'ull', # Ultra Low Latency - '-zerolatency', '1', - '-g', '30', # Keyframe every 1s - '-bf', '0', # No B-frames - '-f', self.stream_fmt, - '-' # Output to pipe - ] + print(f"Error creating codec {codec_name}: {e}") + self.codec = None + def decode(self, binary_data): + if not self.codec: + return None + try: - self.process = subprocess.Popen( - self.ffmpeg_cmd, - stdin=subprocess.PIPE, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - bufsize=0 # Unbuffered IO - ) - except FileNotFoundError: - self.get_logger().error("FFmpeg not found. Ensure ffmpeg is installed.") + packets = self.codec.parse(binary_data) + for packet in packets: + frames = self.codec.decode(packet) + for frame in frames: + return frame.to_ndarray(format='rgb24') + except Exception as e: + print(f"Decode error: {e}") + return None + +# -------------------- ROS Node -------------------- + +class UserInterfaceNode(Node): + def __init__(self): + super().__init__('multi_camera_qt_interface') + self.bridge = CvBridge() + self.subscriptions_map = {} + self.image_signals = {} + self.decoders = {} + self.last_frame_time = {} + self.max_fps = 60.0 + + def subscribe(self, topic, msg_type_str): + if topic in self.subscriptions_map: + return + + qos = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1 + ) + self.last_frame_time[topic] = 0.0 + + # Map string type to class type + if 'CompressedImage' in str(msg_type_str): + msg_type = CompressedImage + else: + msg_type = Image + + self.subscriptions_map[topic] = self.create_subscription( + msg_type, + topic, + lambda msg, t=topic: self.callback(msg, t), + qos + ) + + def unsubscribe(self, topic): + if topic in self.subscriptions_map: + self.destroy_subscription(self.subscriptions_map[topic]) + del self.subscriptions_map[topic] + del self.last_frame_time[topic] + if topic in self.decoders: + del self.decoders[topic] + + def callback(self, msg, topic): + if topic not in self.image_signals: return + + now = time.time() + if now - self.last_frame_time.get(topic, 0.0) < 1.0 / self.max_fps: + return + self.last_frame_time[topic] = now + + cv_img = None + try: + if isinstance(msg, CompressedImage): + if topic not in self.decoders: + fmt = msg.format.lower() + codec = 'hevc' if 'hevc' in fmt else 'av1' if 'av1' in fmt else 'h264' + self.decoders[topic] = StreamDecoder(codec) + + cv_img = self.decoders[topic].decode(bytes(msg.data)) + + elif isinstance(msg, Image): + if "av1" in msg.encoding.lower(): + if topic not in self.decoders: + self.decoders[topic] = StreamDecoder('av1') + cv_img = self.decoders[topic].decode(bytes(msg.data)) + else: + cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') + + if cv_img is not None: + signal = self.image_signals.get(topic) + if signal: + signal.image_ready.emit(cv_img) + + except Exception as e: + self.get_logger().error(f"Processing error on {topic}: {e}") + +# -------------------- UI Components -------------------- + +class CameraWidget(QWidget): + def __init__(self, topic): + super().__init__() + self.topic = topic + self.last_frame = None + + self.label = QLabel("Waiting for Stream...") + self.label.setAlignment(Qt.AlignCenter) + self.label.setStyleSheet("background-color: #000; color: #555;") - self.publisher_ = self.create_publisher(CompressedImage, self.topic_name, 10) + # FIX 1: Use QSizePolicy instead of QWidget.SizePolicy + self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) - # 3. Start Threads - self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) + title = QLabel(topic) + title.setAlignment(Qt.AlignCenter) + title.setStyleSheet("font-size: 10px; color: #888; background: #1a1a1a; padding: 2px;") + + layout = QVBoxLayout(self) + layout.setContentsMargins(0, 0, 0, 0) + layout.setSpacing(0) + layout.addWidget(self.label, 1) + layout.addWidget(title) + + def update_image(self, cv_img): + self.last_frame = cv_img + h, w, ch = cv_img.shape + bytes_per_line = ch * w - self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) - self.output_thread.start() + qimg = QImage(cv_img.data, w, h, bytes_per_line, QImage.Format_RGB888).copy() - self.get_logger().info(f"Streaming {self.width}x{self.height} via {self.codec} (MJPG Input)...") + pix = QPixmap.fromImage(qimg) + self.label.setPixmap(pix.scaled( + self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation + )) - def initalize_parameters(self): - # Define the parameter descriptor to specify the type - topic_parameter_descriptor = ParameterDescriptor( - type=ParameterType.PARAMETER_STRING, - description='A wecam topic to publish to.' - ) + def resizeEvent(self, event): + if self.last_frame is not None: + self.update_image(self.last_frame) + super().resizeEvent(event) + +class MultiCameraWindow(QMainWindow): + def __init__(self, ros_node): + super().__init__() + self.ros_node = ros_node + self.camera_widgets = {} + self.setWindowTitle("ROS2 Stream Viewer") + + main_widget = QWidget() + self.layout = QHBoxLayout(main_widget) - camera_id_parameter_descriptor = ParameterDescriptor( - type=ParameterType.PARAMETER_STRING, - description='A wecam id to read from.' - ) + side_panel = QWidget() + side_layout = QVBoxLayout(side_panel) + self.topic_list = QListWidget() + self.topic_list.itemChanged.connect(self.topic_toggled) + + btn_refresh = QPushButton("Refresh Topics") + btn_refresh.clicked.connect(self.refresh_topics) + + side_layout.addWidget(QLabel("Available Topics:")) + side_layout.addWidget(self.topic_list) + side_layout.addWidget(btn_refresh) + + self.grid_widget = QWidget() + self.grid_layout = QGridLayout(self.grid_widget) + self.grid_layout.setContentsMargins(0,0,0,0) + + self.layout.addWidget(side_panel, 1) + self.layout.addWidget(self.grid_widget, 4) + self.setCentralWidget(main_widget) + + self.refresh_topics() - self.declare_parameter( - 'topic_name', - value='', - descriptor=topic_parameter_descriptor - ) + def refresh_topics(self): + self.topic_list.blockSignals(True) + checked = {self.topic_list.item(i).text() for i in range(self.topic_list.count()) + if self.topic_list.item(i).checkState() == Qt.Checked} + + self.topic_list.clear() - self.declare_parameter( - 'device_id', - value='0', - descriptor=camera_id_parameter_descriptor - ) + try: + topic_names_and_types = self.ros_node.get_topic_names_and_types() + except Exception as e: + print(f"Error fetching topics: {e}") + topic_names_and_types = [] + + for name, types in topic_names_and_types: + # Check for relevant message types + if any(t in ['sensor_msgs/msg/Image', 'sensor_msgs/msg/CompressedImage'] for t in types): + item = QListWidgetItem(name) + item.setFlags(item.flags() | Qt.ItemIsUserCheckable) + item.setCheckState(Qt.Checked if name in checked else Qt.Unchecked) + # Store the type string for later use + item.setData(Qt.UserRole, types[0]) + self.topic_list.addItem(item) - def feed_encoder(self): - """Capture frame and push to FFmpeg stdin""" - if not self.cap.isOpened(): - return - - ret, frame = self.cap.read() - if ret: - # Resize if the camera ignores our resolution request - if frame.shape[1] != self.width or frame.shape[0] != self.height: - frame = cv2.resize(frame, (self.width, self.height)) - - try: - self.process.stdin.write(frame.tobytes()) - self.process.stdin.flush() - except BrokenPipeError: - self.get_logger().error("FFmpeg stdin broken pipe") - rclpy.shutdown() + self.topic_list.blockSignals(False) + + def topic_toggled(self, item): + topic = item.text() + msg_type = item.data(Qt.UserRole) + + if item.checkState() == Qt.Checked: + self.add_camera(topic, msg_type) else: - self.get_logger().warn("Camera frame read failed.") + self.remove_camera(topic) - def read_encoded_stream(self): - """Continuously read from FFmpeg stdout and publish""" - # Buffer size large enough for 1080p frames to prevent artifacting - chunk_size = 256 * 1024 + def add_camera(self, topic, msg_type): + if topic in self.camera_widgets: return - while rclpy.ok() and self.process.poll() is None: - try: - # Read whatever is available - data = self.process.stdout.read(chunk_size) - - if data: - msg = CompressedImage() - msg.header.stamp = self.get_clock().now().to_msg() - msg.format = self.stream_fmt - msg.data = data - - self.publisher_.publish(msg) - else: - time.sleep(0.002) - except Exception as e: - self.get_logger().error(f"Read error: {e}") - break - - def destroy_node(self): - if self.process: - self.process.kill() - self.cap.release() - super().destroy_node() - -def main(args=None): - rclpy.init(args=args) - node = VideoCompressionNode() + widget = CameraWidget(topic) + sig = ImageSignal() + sig.image_ready.connect(widget.update_image) + self.ros_node.image_signals[topic] = sig + + self.ros_node.subscribe(topic, msg_type) + self.camera_widgets[topic] = widget + self.rebuild_grid() + + def remove_camera(self, topic): + if topic in self.camera_widgets: + self.ros_node.unsubscribe(topic) + if topic in self.ros_node.image_signals: + del self.ros_node.image_signals[topic] + + widget = self.camera_widgets.pop(topic) + self.grid_layout.removeWidget(widget) + widget.deleteLater() + self.rebuild_grid() + + def rebuild_grid(self): + for i in reversed(range(self.grid_layout.count())): + item = self.grid_layout.itemAt(i) + if item.widget(): + item.widget().setParent(None) + + widgets = list(self.camera_widgets.values()) + if not widgets: return + + count = len(widgets) + cols = math.ceil(math.sqrt(count)) + + for i, widget in enumerate(widgets): + row, col = divmod(i, cols) + self.grid_layout.addWidget(widget, row, col) + +# -------------------- Execution -------------------- + +def main(): + rclpy.init() + + node = UserInterfaceNode() + ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + ros_thread.start() + + app = QApplication(sys.argv) + app.setStyleSheet(dark_stylesheet) + + gui = MultiCameraWindow(node) + gui.resize(1280, 720) + gui.show() + try: - rclpy.spin(node) - except KeyboardInterrupt: - pass + # FIX 2: Do not wrap in sys.exit() yet to allow clean shutdown block + app.exec() finally: + # FIX 3: Check if rclpy is still okay before shutting down node.destroy_node() if rclpy.ok(): rclpy.shutdown() -if __name__ == '__main__': - main() +if __name__ == "__main__": + main() \ No newline at end of file From 7605bfcba1d23e56e05a16ba31c5a43fc3c404f9 Mon Sep 17 00:00:00 2001 From: Adam Yaj Date: Tue, 3 Mar 2026 19:29:24 -0600 Subject: [PATCH 09/21] Correctly revert the user interface instead of changing the compression --- .../camera_interface/compression.py | 448 +++++++----------- .../camera_interface/qt_user_interface.py | 395 ++++++++------- 2 files changed, 351 insertions(+), 492 deletions(-) diff --git a/src/camera_interface/camera_interface/compression.py b/src/camera_interface/camera_interface/compression.py index 9d107e48..a22d49e4 100644 --- a/src/camera_interface/camera_interface/compression.py +++ b/src/camera_interface/camera_interface/compression.py @@ -1,313 +1,189 @@ #!/usr/bin/env python3 -import sys -import threading -import time -import math -import numpy as np -import av # PyAV - import rclpy from rclpy.node import Node -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy -from sensor_msgs.msg import Image, CompressedImage -from cv_bridge import CvBridge - -from PySide6.QtWidgets import ( - QApplication, QMainWindow, QLabel, QPushButton, - QVBoxLayout, QHBoxLayout, QWidget, - QListWidget, QListWidgetItem, - QGridLayout, QSizePolicy # <--- Added QSizePolicy import -) -from PySide6.QtGui import QImage, QPixmap -from PySide6.QtCore import Qt, Signal, QObject - -# -------------------- Dark Theme -------------------- - -dark_stylesheet = """ -QWidget { background-color: #121212; color: #e0e0e0; } -QPushButton { background-color: #1f1f1f; border: 1px solid #333; padding: 6px; } -QPushButton:hover { background-color: #333; } -QListWidget { background-color: #1c1c1c; border: 1px solid #333; } -""" - -# -------------------- Decoding Logic -------------------- - -class ImageSignal(QObject): - image_ready = Signal(object) - -class StreamDecoder: - """Stateful decoder using PyAV for AV1/HEVC/H264 streams.""" - def __init__(self, codec_name='av1'): - try: - self.codec = av.CodecContext.create(codec_name, 'r') - except Exception as e: - print(f"Error creating codec {codec_name}: {e}") - self.codec = None - - def decode(self, binary_data): - if not self.codec: - return None - - try: - packets = self.codec.parse(binary_data) - for packet in packets: - frames = self.codec.decode(packet) - for frame in frames: - return frame.to_ndarray(format='rgb24') - except Exception as e: - print(f"Decode error: {e}") - return None - -# -------------------- ROS Node -------------------- +from rcl_interfaces.msg import ParameterDescriptor, ParameterType +from sensor_msgs.msg import CompressedImage +import cv2 +import subprocess +import threading +import time -class UserInterfaceNode(Node): +class VideoCompressionNode(Node): def __init__(self): - super().__init__('multi_camera_qt_interface') - self.bridge = CvBridge() - self.subscriptions_map = {} - self.image_signals = {} - self.decoders = {} - self.last_frame_time = {} - self.max_fps = 60.0 - - def subscribe(self, topic, msg_type_str): - if topic in self.subscriptions_map: - return - - qos = QoSProfile( - reliability=QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1 - ) - self.last_frame_time[topic] = 0.0 - - # Map string type to class type - if 'CompressedImage' in str(msg_type_str): - msg_type = CompressedImage - else: - msg_type = Image - - self.subscriptions_map[topic] = self.create_subscription( - msg_type, - topic, - lambda msg, t=topic: self.callback(msg, t), - qos - ) - - def unsubscribe(self, topic): - if topic in self.subscriptions_map: - self.destroy_subscription(self.subscriptions_map[topic]) - del self.subscriptions_map[topic] - del self.last_frame_time[topic] - if topic in self.decoders: - del self.decoders[topic] - - def callback(self, msg, topic): - if topic not in self.image_signals: - return - - now = time.time() - if now - self.last_frame_time.get(topic, 0.0) < 1.0 / self.max_fps: - return - self.last_frame_time[topic] = now - - cv_img = None - try: - if isinstance(msg, CompressedImage): - if topic not in self.decoders: - fmt = msg.format.lower() - codec = 'hevc' if 'hevc' in fmt else 'av1' if 'av1' in fmt else 'h264' - self.decoders[topic] = StreamDecoder(codec) - - cv_img = self.decoders[topic].decode(bytes(msg.data)) - - elif isinstance(msg, Image): - if "av1" in msg.encoding.lower(): - if topic not in self.decoders: - self.decoders[topic] = StreamDecoder('av1') - cv_img = self.decoders[topic].decode(bytes(msg.data)) - else: - cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') - - if cv_img is not None: - signal = self.image_signals.get(topic) - if signal: - signal.image_ready.emit(cv_img) - - except Exception as e: - self.get_logger().error(f"Processing error on {topic}: {e}") - -# -------------------- UI Components -------------------- - -class CameraWidget(QWidget): - def __init__(self, topic): - super().__init__() - self.topic = topic - self.last_frame = None - - self.label = QLabel("Waiting for Stream...") - self.label.setAlignment(Qt.AlignCenter) - self.label.setStyleSheet("background-color: #000; color: #555;") - - # FIX 1: Use QSizePolicy instead of QWidget.SizePolicy - self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) + super().__init__('video_compression_node') - title = QLabel(topic) - title.setAlignment(Qt.AlignCenter) - title.setStyleSheet("font-size: 10px; color: #888; background: #1a1a1a; padding: 2px;") - - layout = QVBoxLayout(self) - layout.setContentsMargins(0, 0, 0, 0) - layout.setSpacing(0) - layout.addWidget(self.label, 1) - layout.addWidget(title) - - def update_image(self, cv_img): - self.last_frame = cv_img - h, w, ch = cv_img.shape - bytes_per_line = ch * w + self.initalize_parameters() - qimg = QImage(cv_img.data, w, h, bytes_per_line, QImage.Format_RGB888).copy() + # Get the parameter value + self.topic_name = self.get_parameter('topic_name').get_parameter_value().string_value + self.device_id = self.get_parameter('device_id').get_parameter_value().string_value - pix = QPixmap.fromImage(qimg) - self.label.setPixmap(pix.scaled( - self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation - )) - - def resizeEvent(self, event): - if self.last_frame is not None: - self.update_image(self.last_frame) - super().resizeEvent(event) - -class MultiCameraWindow(QMainWindow): - def __init__(self, ros_node): - super().__init__() - self.ros_node = ros_node - self.camera_widgets = {} - self.setWindowTitle("ROS2 Stream Viewer") + # Conversion Logic + try: + # If the string is just a number (e.g., "0"), convert to int + if str(self.device_id).isdigit(): + self.camera_source = int(self.device_id) + else: + # If it's a path (e.g., "/dev/video0"), keep it as a string + self.camera_source = str(self.device_id) + except Exception as e: + self.get_logger().error(f"Invalid device_id: {e}") + + self.get_logger().info(f"Capturing \"{self.camera_source}\" : \"{self.topic_name}\"") + + self.fps = 30 + + # 1. Setup Camera with V4L2 Backend + # 'cv2.CAP_V4L2' prevents the GStreamer "Internal data stream error" + self.cap = cv2.VideoCapture(self.camera_source, cv2.CAP_V4L2) + + # CRITICAL: Force MJPEG. + # Most USB cams cannot do 1280x720 @ 30fps in raw YUYV format (bandwidth limit). + self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G')) + + # Force Resolution + self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + self.cap.set(cv2.CAP_PROP_FPS, self.fps) + + # Read actual parameters (in case camera rejected our request) + w = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 640 + h = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 480 + + # NVENC requires even dimensions + self.width = int(w) if int(w) % 2 == 0 else int(w) - 1 + self.height = int(h) if int(h) % 2 == 0 else int(h) - 1 + + # Codec Settings + self.codec = 'hevc_nvenc' # NVIDIA Hardware Encoder + self.stream_fmt = 'hevc' # Raw HEVC stream + + # 2. Setup FFmpeg + self.ffmpeg_cmd = [ + 'ffmpeg', '-y', + '-hide_banner', '-loglevel', 'error', + '-f', 'rawvideo', + '-vcodec', 'rawvideo', + '-s', f'{self.width}x{self.height}', + '-pix_fmt', 'bgr24', + '-r', str(self.fps), + '-i', '-', # Input from pipe + '-c:v', self.codec, + '-preset', 'p1', # Low latency preset + '-tune', 'ull', # Ultra Low Latency + '-zerolatency', '1', + '-g', '30', # Keyframe every 1s + '-bf', '0', # No B-frames + '-f', self.stream_fmt, + '-' # Output to pipe + ] - main_widget = QWidget() - self.layout = QHBoxLayout(main_widget) - - side_panel = QWidget() - side_layout = QVBoxLayout(side_panel) - self.topic_list = QListWidget() - self.topic_list.itemChanged.connect(self.topic_toggled) - - btn_refresh = QPushButton("Refresh Topics") - btn_refresh.clicked.connect(self.refresh_topics) + try: + self.process = subprocess.Popen( + self.ffmpeg_cmd, + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + bufsize=0 # Unbuffered IO + ) + except FileNotFoundError: + self.get_logger().error("FFmpeg not found. Ensure ffmpeg is installed.") + return - side_layout.addWidget(QLabel("Available Topics:")) - side_layout.addWidget(self.topic_list) - side_layout.addWidget(btn_refresh) + self.publisher_ = self.create_publisher(CompressedImage, self.topic_name, 10) - self.grid_widget = QWidget() - self.grid_layout = QGridLayout(self.grid_widget) - self.grid_layout.setContentsMargins(0,0,0,0) + # 3. Start Threads + self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) - self.layout.addWidget(side_panel, 1) - self.layout.addWidget(self.grid_widget, 4) - self.setCentralWidget(main_widget) + self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) + self.output_thread.start() - self.refresh_topics() + self.get_logger().info(f"Streaming {self.width}x{self.height} via {self.codec} (MJPG Input)...") - def refresh_topics(self): - self.topic_list.blockSignals(True) - checked = {self.topic_list.item(i).text() for i in range(self.topic_list.count()) - if self.topic_list.item(i).checkState() == Qt.Checked} - - self.topic_list.clear() - - try: - topic_names_and_types = self.ros_node.get_topic_names_and_types() - except Exception as e: - print(f"Error fetching topics: {e}") - topic_names_and_types = [] - - for name, types in topic_names_and_types: - # Check for relevant message types - if any(t in ['sensor_msgs/msg/Image', 'sensor_msgs/msg/CompressedImage'] for t in types): - item = QListWidgetItem(name) - item.setFlags(item.flags() | Qt.ItemIsUserCheckable) - item.setCheckState(Qt.Checked if name in checked else Qt.Unchecked) - # Store the type string for later use - item.setData(Qt.UserRole, types[0]) - self.topic_list.addItem(item) - - self.topic_list.blockSignals(False) - - def topic_toggled(self, item): - topic = item.text() - msg_type = item.data(Qt.UserRole) + def initalize_parameters(self): + # Define the parameter descriptor to specify the type + topic_parameter_descriptor = ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description='A wecam topic to publish to.' + ) - if item.checkState() == Qt.Checked: - self.add_camera(topic, msg_type) - else: - self.remove_camera(topic) + camera_id_parameter_descriptor = ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description='A wecam id to read from.' + ) - def add_camera(self, topic, msg_type): - if topic in self.camera_widgets: return + self.declare_parameter( + 'topic_name', + value='', + descriptor=topic_parameter_descriptor + ) - widget = CameraWidget(topic) - sig = ImageSignal() - sig.image_ready.connect(widget.update_image) - self.ros_node.image_signals[topic] = sig + self.declare_parameter( + 'device_id', + value='0', + descriptor=camera_id_parameter_descriptor + ) - self.ros_node.subscribe(topic, msg_type) - self.camera_widgets[topic] = widget - self.rebuild_grid() - - def remove_camera(self, topic): - if topic in self.camera_widgets: - self.ros_node.unsubscribe(topic) - if topic in self.ros_node.image_signals: - del self.ros_node.image_signals[topic] + def feed_encoder(self): + """Capture frame and push to FFmpeg stdin""" + if not self.cap.isOpened(): + return - widget = self.camera_widgets.pop(topic) - self.grid_layout.removeWidget(widget) - widget.deleteLater() - self.rebuild_grid() - - def rebuild_grid(self): - for i in reversed(range(self.grid_layout.count())): - item = self.grid_layout.itemAt(i) - if item.widget(): - item.widget().setParent(None) - - widgets = list(self.camera_widgets.values()) - if not widgets: return + ret, frame = self.cap.read() + if ret: + # Resize if the camera ignores our resolution request + if frame.shape[1] != self.width or frame.shape[0] != self.height: + frame = cv2.resize(frame, (self.width, self.height)) + + try: + self.process.stdin.write(frame.tobytes()) + self.process.stdin.flush() + except BrokenPipeError: + self.get_logger().error("FFmpeg stdin broken pipe") + rclpy.shutdown() + else: + self.get_logger().warn("Camera frame read failed.") - count = len(widgets) - cols = math.ceil(math.sqrt(count)) + def read_encoded_stream(self): + """Continuously read from FFmpeg stdout and publish""" + # Buffer size large enough for 1080p frames to prevent artifacting + chunk_size = 256 * 1024 - for i, widget in enumerate(widgets): - row, col = divmod(i, cols) - self.grid_layout.addWidget(widget, row, col) - -# -------------------- Execution -------------------- - -def main(): - rclpy.init() - - node = UserInterfaceNode() - ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) - ros_thread.start() - - app = QApplication(sys.argv) - app.setStyleSheet(dark_stylesheet) - - gui = MultiCameraWindow(node) - gui.resize(1280, 720) - gui.show() - + while rclpy.ok() and self.process.poll() is None: + try: + # Read whatever is available + data = self.process.stdout.read(chunk_size) + + if data: + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.format = self.stream_fmt + msg.data = data + + self.publisher_.publish(msg) + else: + time.sleep(0.002) + except Exception as e: + self.get_logger().error(f"Read error: {e}") + break + + def destroy_node(self): + if self.process: + self.process.kill() + self.cap.release() + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = VideoCompressionNode() try: - # FIX 2: Do not wrap in sys.exit() yet to allow clean shutdown block - app.exec() + rclpy.spin(node) + except KeyboardInterrupt: + pass finally: - # FIX 3: Check if rclpy is still okay before shutting down node.destroy_node() if rclpy.ok(): rclpy.shutdown() -if __name__ == "__main__": - main() \ No newline at end of file +if __name__ == '__main__': + main() diff --git a/src/camera_interface/camera_interface/qt_user_interface.py b/src/camera_interface/camera_interface/qt_user_interface.py index 10cfb9fe..d224148d 100644 --- a/src/camera_interface/camera_interface/qt_user_interface.py +++ b/src/camera_interface/camera_interface/qt_user_interface.py @@ -2,19 +2,20 @@ import threading import time import math +import numpy as np +import av # PyAV import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy -from sensor_msgs.msg import Image +from sensor_msgs.msg import Image, CompressedImage from cv_bridge import CvBridge -import cv2 from PySide6.QtWidgets import ( QApplication, QMainWindow, QLabel, QPushButton, QVBoxLayout, QHBoxLayout, QWidget, QListWidget, QListWidgetItem, - QGridLayout, QSizePolicy + QGridLayout, QSizePolicy # <--- Added QSizePolicy import ) from PySide6.QtGui import QImage, QPixmap from PySide6.QtCore import Qt, Signal, QObject @@ -22,38 +23,54 @@ # -------------------- Dark Theme -------------------- dark_stylesheet = """ -QWidget { - background-color: #121212; - color: #e0e0e0; -} -QPushButton { - background-color: #1f1f1f; - border: 1px solid #333; - padding: 6px; -} -QListWidget { - background-color: #1c1c1c; -} +QWidget { background-color: #121212; color: #e0e0e0; } +QPushButton { background-color: #1f1f1f; border: 1px solid #333; padding: 6px; } +QPushButton:hover { background-color: #333; } +QListWidget { background-color: #1c1c1c; border: 1px solid #333; } """ -# -------------------- Qt Signal -------------------- +# -------------------- Decoding Logic -------------------- class ImageSignal(QObject): image_ready = Signal(object) +class StreamDecoder: + """Stateful decoder using PyAV for AV1/HEVC/H264 streams.""" + def __init__(self, codec_name='av1'): + try: + self.codec = av.CodecContext.create(codec_name, 'r') + except Exception as e: + print(f"Error creating codec {codec_name}: {e}") + self.codec = None + + def decode(self, binary_data): + if not self.codec: + return None + + try: + packets = self.codec.parse(binary_data) + for packet in packets: + frames = self.codec.decode(packet) + for frame in frames: + return frame.to_ndarray(format='rgb24') + except Exception as e: + print(f"Decode error: {e}") + return None + # -------------------- ROS Node -------------------- class UserInterfaceNode(Node): def __init__(self): super().__init__('multi_camera_qt_interface') self.bridge = CvBridge() - self.camera_subscriptions = {} + self.subscriptions_map = {} self.image_signals = {} - self.last_frame = {} - self.max_fps = 30.0 + self.decoders = {} + self.last_frame_time = {} + self.max_fps = 60.0 - def subscribe(self, topic): - if topic in self.camera_subscriptions: + def subscribe(self, topic, msg_type_str): + if topic in self.subscriptions_map: return qos = QoSProfile( @@ -61,269 +78,235 @@ def subscribe(self, topic): history=QoSHistoryPolicy.KEEP_LAST, depth=1 ) + self.last_frame_time[topic] = 0.0 - self.last_frame[topic] = 0.0 + # Map string type to class type + if 'CompressedImage' in str(msg_type_str): + msg_type = CompressedImage + else: + msg_type = Image - self.camera_subscriptions[topic] = self.create_subscription( - Image, + self.subscriptions_map[topic] = self.create_subscription( + msg_type, topic, lambda msg, t=topic: self.callback(msg, t), qos ) def unsubscribe(self, topic): - if topic in self.camera_subscriptions: - self.destroy_subscription(self.camera_subscriptions[topic]) - del self.camera_subscriptions[topic] - del self.last_frame[topic] - del self.image_signals[topic] + if topic in self.subscriptions_map: + self.destroy_subscription(self.subscriptions_map[topic]) + del self.subscriptions_map[topic] + del self.last_frame_time[topic] + if topic in self.decoders: + del self.decoders[topic] def callback(self, msg, topic): - # Topic removed while callback was queued if topic not in self.image_signals: return now = time.time() - if now - self.last_frame.get(topic, 0.0) < 1.0 / self.max_fps: + if now - self.last_frame_time.get(topic, 0.0) < 1.0 / self.max_fps: return - - self.last_frame[topic] = now - - cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') - - signal = self.image_signals.get(topic) - if signal is not None: - signal.image_ready.emit(cv_img) - -# -------------------- Camera Widget -------------------- + self.last_frame_time[topic] = now + + cv_img = None + try: + if isinstance(msg, CompressedImage): + if topic not in self.decoders: + fmt = msg.format.lower() + codec = 'hevc' if 'hevc' in fmt else 'av1' if 'av1' in fmt else 'h264' + self.decoders[topic] = StreamDecoder(codec) + + cv_img = self.decoders[topic].decode(bytes(msg.data)) + + elif isinstance(msg, Image): + if "av1" in msg.encoding.lower(): + if topic not in self.decoders: + self.decoders[topic] = StreamDecoder('av1') + cv_img = self.decoders[topic].decode(bytes(msg.data)) + else: + cv_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8') + + if cv_img is not None: + signal = self.image_signals.get(topic) + if signal: + signal.image_ready.emit(cv_img) + + except Exception as e: + self.get_logger().error(f"Processing error on {topic}: {e}") + +# -------------------- UI Components -------------------- class CameraWidget(QWidget): def __init__(self, topic): super().__init__() self.topic = topic + self.last_frame = None - self.label = QLabel("Waiting for image...") + self.label = QLabel("Waiting for Stream...") self.label.setAlignment(Qt.AlignCenter) - self.label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Ignored) - self.label.setMinimumSize(1, 1) - + self.label.setStyleSheet("background-color: #000; color: #555;") + + # FIX 1: Use QSizePolicy instead of QWidget.SizePolicy + self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) + title = QLabel(topic) title.setAlignment(Qt.AlignCenter) - title.setFixedHeight(18) - title.setStyleSheet("font-size:10px; border-top:1px solid #333") - - # ---- Image container ---- - image_container = QWidget() - image_layout = QVBoxLayout() - image_layout.setContentsMargins(0, 0, 0, 0) - image_layout.addWidget(self.label) - # image_layout.addWidget(title, 0) - image_container.setLayout(image_layout) - - image_container.setSizePolicy( - QSizePolicy.Expanding, - QSizePolicy.Ignored - ) - - # ---- Main layout ---- - layout = QVBoxLayout() + title.setStyleSheet("font-size: 10px; color: #888; background: #1a1a1a; padding: 2px;") + + layout = QVBoxLayout(self) layout.setContentsMargins(0, 0, 0, 0) layout.setSpacing(0) - - layout.addWidget(title, 1) # stays attached - layout.addWidget(image_container, 0) # stretches - - self.setLayout(layout) + layout.addWidget(self.label, 1) + layout.addWidget(title) def update_image(self, cv_img): - self.last_frame = cv_img # cache frame - + self.last_frame = cv_img h, w, ch = cv_img.shape - qimg = QImage(cv_img.data, w, h, ch * w, QImage.Format_RGB888) + bytes_per_line = ch * w + + qimg = QImage(cv_img.data, w, h, bytes_per_line, QImage.Format_RGB888).copy() + pix = QPixmap.fromImage(qimg) - - self.label.setPixmap( - pix.scaled( - self.label.size(), - Qt.KeepAspectRatio, - Qt.SmoothTransformation - ) - ) + self.label.setPixmap(pix.scaled( + self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation + )) def resizeEvent(self, event): - if hasattr(self, "last_frame"): + if self.last_frame is not None: self.update_image(self.last_frame) - -# -------------------- Main Window -------------------- + super().resizeEvent(event) class MultiCameraWindow(QMainWindow): def __init__(self, ros_node): super().__init__() - self.setWindowTitle("ROS2 Multi-Camera Viewer") self.ros_node = ros_node - self.camera_widgets = {} + self.setWindowTitle("ROS2 Stream Viewer") main_widget = QWidget() - main_layout = QHBoxLayout() - - # ---- Sidebar ---- - side_layout = QVBoxLayout() - self.refresh_btn = QPushButton("Refresh Topics") - self.refresh_btn.clicked.connect(self.refresh_topics) - + self.layout = QHBoxLayout(main_widget) + + side_panel = QWidget() + side_layout = QVBoxLayout(side_panel) self.topic_list = QListWidget() self.topic_list.itemChanged.connect(self.topic_toggled) - + + btn_refresh = QPushButton("Refresh Topics") + btn_refresh.clicked.connect(self.refresh_topics) + + side_layout.addWidget(QLabel("Available Topics:")) side_layout.addWidget(self.topic_list) - side_layout.addWidget(self.refresh_btn) - - # ---- Grid Area ---- + side_layout.addWidget(btn_refresh) + self.grid_widget = QWidget() - self.grid_layout = QGridLayout() - self.grid_widget.setLayout(self.grid_layout) - self.grid_widget.setSizePolicy( - QSizePolicy.Expanding, - QSizePolicy.Expanding - ) - - - main_layout.addLayout(side_layout) - main_layout.addWidget(self.grid_widget, 1) - - main_widget.setLayout(main_layout) + self.grid_layout = QGridLayout(self.grid_widget) + self.grid_layout.setContentsMargins(0,0,0,0) + + self.layout.addWidget(side_panel, 1) + self.layout.addWidget(self.grid_widget, 4) self.setCentralWidget(main_widget) - + self.refresh_topics() - # -------------------- - def refresh_topics(self): self.topic_list.blockSignals(True) - self.topic_list.clear() + checked = {self.topic_list.item(i).text() for i in range(self.topic_list.count()) + if self.topic_list.item(i).checkState() == Qt.Checked} - topics = self.ros_node.get_topic_names_and_types() - for name, types in topics: - if "sensor_msgs/msg/Image" in types: + self.topic_list.clear() + + try: + topic_names_and_types = self.ros_node.get_topic_names_and_types() + except Exception as e: + print(f"Error fetching topics: {e}") + topic_names_and_types = [] + + for name, types in topic_names_and_types: + # Check for relevant message types + if any(t in ['sensor_msgs/msg/Image', 'sensor_msgs/msg/CompressedImage'] for t in types): item = QListWidgetItem(name) item.setFlags(item.flags() | Qt.ItemIsUserCheckable) - item.setCheckState(Qt.Unchecked) + item.setCheckState(Qt.Checked if name in checked else Qt.Unchecked) + # Store the type string for later use + item.setData(Qt.UserRole, types[0]) self.topic_list.addItem(item) - + self.topic_list.blockSignals(False) - # -------------------- - def topic_toggled(self, item): topic = item.text() - checked = item.checkState() == Qt.Checked - - if checked: - self.add_camera(topic) + msg_type = item.data(Qt.UserRole) + + if item.checkState() == Qt.Checked: + self.add_camera(topic, msg_type) else: self.remove_camera(topic) - # -------------------- - - def add_camera(self, topic): - if topic in self.camera_widgets: - return - + def add_camera(self, topic, msg_type): + if topic in self.camera_widgets: return + widget = CameraWidget(topic) - signal = ImageSignal() - signal.image_ready.connect(widget.update_image) - - self.ros_node.image_signals[topic] = signal - self.ros_node.subscribe(topic) - + sig = ImageSignal() + sig.image_ready.connect(widget.update_image) + self.ros_node.image_signals[topic] = sig + + self.ros_node.subscribe(topic, msg_type) self.camera_widgets[topic] = widget self.rebuild_grid() def remove_camera(self, topic): - if topic not in self.camera_widgets: - return - - self.ros_node.unsubscribe(topic) - - widget = self.camera_widgets[topic] - widget.setParent(None) - widget.deleteLater() - - del self.camera_widgets[topic] - self.rebuild_grid() - + if topic in self.camera_widgets: + self.ros_node.unsubscribe(topic) + if topic in self.ros_node.image_signals: + del self.ros_node.image_signals[topic] + + widget = self.camera_widgets.pop(topic) + self.grid_layout.removeWidget(widget) + widget.deleteLater() + self.rebuild_grid() - # -------------------- - def rebuild_grid(self): - # Clear previous stretches to prevent "dead space" - for i in range(self.grid_layout.rowCount()): - self.grid_layout.setRowStretch(i, 0) - for i in range(self.grid_layout.columnCount()): - self.grid_layout.setColumnStretch(i, 0) - - # Properly clear the layout - while self.grid_layout.count(): - item = self.grid_layout.takeAt(0) + for i in reversed(range(self.grid_layout.count())): + item = self.grid_layout.itemAt(i) if item.widget(): - item.widget().hide() - - widgets = list(self.camera_widgets.values()) - count = len(widgets) + item.widget().setParent(None) - if count == 0: - return + widgets = list(self.camera_widgets.values()) + if not widgets: return + count = len(widgets) cols = math.ceil(math.sqrt(count)) - rows = math.ceil(count / cols) - - index = 0 - for r in range(rows): - for c in range(cols): - if index >= count: - break - - widget = widgets[index] - self.grid_layout.addWidget(widget, r, c) - - # Ensure it's visible and active - widget.setVisible(True) - widget.update() # Triggers a repaint of the camera frame - - index += 1 - - for r in range(rows): - self.grid_layout.setRowStretch(r, 1) - for c in range(cols): - self.grid_layout.setColumnStretch(c, 1) - - self.grid_layout.setContentsMargins(0, 0, 0, 0) - self.grid_layout.setSpacing(1) + for i, widget in enumerate(widgets): + row, col = divmod(i, cols) + self.grid_layout.addWidget(widget, row, col) - -# -------------------- Main -------------------- +# -------------------- Execution -------------------- def main(): rclpy.init() - + + node = UserInterfaceNode() + ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + ros_thread.start() + app = QApplication(sys.argv) app.setStyleSheet(dark_stylesheet) - - ros_node = UserInterfaceNode() - - ros_thread = threading.Thread( - target=rclpy.spin, - args=(ros_node,), - daemon=True - ) - ros_thread.start() - - win = MultiCameraWindow(ros_node) - # win.resize(1200, 800) - win.show() - - sys.exit(app.exec()) + + gui = MultiCameraWindow(node) + gui.resize(1280, 720) + gui.show() + + try: + # FIX 2: Do not wrap in sys.exit() yet to allow clean shutdown block + app.exec() + finally: + # FIX 3: Check if rclpy is still okay before shutting down + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() if __name__ == "__main__": - main() + main() \ No newline at end of file From da0c10dbf0daca603fd5d89497fcf4e8a852b2bc Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Tue, 3 Mar 2026 19:45:33 -0600 Subject: [PATCH 10/21] Experimenting with different USB ids --- src/camera_interface/launch/camera_system_launch.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/camera_interface/launch/camera_system_launch.py b/src/camera_interface/launch/camera_system_launch.py index 42b07522..386e456a 100644 --- a/src/camera_interface/launch/camera_system_launch.py +++ b/src/camera_interface/launch/camera_system_launch.py @@ -3,14 +3,15 @@ def generate_launch_description(): # NOTE: Instead of an index, you can use the stable path found in /dev/v4l/by-id/ or /dev/v4l/by-path/ + # To get the list of ids, we do : ls -l /dev/v4l/by-id/ # (e.g.) -> /dev/v4l/by-id/usb-Logitech_Webcam_C920_ABC123-video-index0 camera_configs = [ - {"name": "right", "topic": "/webcam/right", "id": "/dev/video6"}, - {"name": "left", "topic": "/webcam/left", "id": "/dev/video8"}, - {"name": "back", "topic": "/webcam/back", "id": "/dev/video0"}, - {"name": "digger", "topic": "/webcam/digger", "id": "/dev/video4"}, - {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/video5"}, - {"name": "front", "topic": "/webcam/front", "id": "/dev/video0"}, + {"name": "right", "topic": "/webcam/right", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index0"}, + {"name": "left", "topic": "/webcam/left", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index0"}, + {"name": "back", "topic": "/webcam/back", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index0"}, + {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index1"}, + {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/video4"}, + {"name": "front", "topic": "/webcam/front", "id": "/dev/video5"}, ] nodes = [ From 3236db69e2ebb464243475a4cf191f792c061fe6 Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Thu, 5 Mar 2026 18:48:07 -0600 Subject: [PATCH 11/21] Add camera reconnect to compression node and add no signal text to the qt interface --- .../camera_interface/compression.py | 220 ++++++++---------- .../camera_interface/qt_user_interface.py | 74 +++++- .../launch/camera_system_launch.py | 6 +- 3 files changed, 164 insertions(+), 136 deletions(-) diff --git a/src/camera_interface/camera_interface/compression.py b/src/camera_interface/camera_interface/compression.py index a22d49e4..45fe3957 100644 --- a/src/camera_interface/camera_interface/compression.py +++ b/src/camera_interface/camera_interface/compression.py @@ -13,164 +13,136 @@ def __init__(self): super().__init__('video_compression_node') self.initalize_parameters() - - # Get the parameter value self.topic_name = self.get_parameter('topic_name').get_parameter_value().string_value self.device_id = self.get_parameter('device_id').get_parameter_value().string_value - # Conversion Logic - try: - # If the string is just a number (e.g., "0"), convert to int - if str(self.device_id).isdigit(): - self.camera_source = int(self.device_id) - else: - # If it's a path (e.g., "/dev/video0"), keep it as a string - self.camera_source = str(self.device_id) - except Exception as e: - self.get_logger().error(f"Invalid device_id: {e}") + # Determine camera source + if self.device_id.isdigit(): + self.camera_source = int(self.device_id) + else: + self.camera_source = self.device_id + + self.fps = 30 + self.cap = None + self.process = None + self.is_running = True - self.get_logger().info(f"Capturing \"{self.camera_source}\" : \"{self.topic_name}\"") + # Codec Settings + self.codec = 'hevc_nvenc' + self.stream_fmt = 'hevc' - self.fps = 30 + self.publisher_ = self.create_publisher(CompressedImage, self.topic_name, 10) - # 1. Setup Camera with V4L2 Backend - # 'cv2.CAP_V4L2' prevents the GStreamer "Internal data stream error" - self.cap = cv2.VideoCapture(self.camera_source, cv2.CAP_V4L2) + # Start the background thread for FFmpeg output + self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) + self.output_thread.start() - # CRITICAL: Force MJPEG. - # Most USB cams cannot do 1280x720 @ 30fps in raw YUYV format (bandwidth limit). - self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G')) + # Timer for capturing frames + self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) - # Force Resolution + self.get_logger().info(f"Node started for device: {self.camera_source}") + + def open_camera(self): + """Attempts to open the camera and configure it.""" + if self.cap is not None: + self.cap.release() + + self.cap = cv2.VideoCapture(self.camera_source, cv2.CAP_V4L2) + if not self.cap.isOpened(): + return False + + self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G')) self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) self.cap.set(cv2.CAP_PROP_FPS, self.fps) - # Read actual parameters (in case camera rejected our request) w = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 640 h = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 480 - - # NVENC requires even dimensions self.width = int(w) if int(w) % 2 == 0 else int(w) - 1 self.height = int(h) if int(h) % 2 == 0 else int(h) - 1 - # Codec Settings - self.codec = 'hevc_nvenc' # NVIDIA Hardware Encoder - self.stream_fmt = 'hevc' # Raw HEVC stream - - # 2. Setup FFmpeg - self.ffmpeg_cmd = [ - 'ffmpeg', '-y', - '-hide_banner', '-loglevel', 'error', - '-f', 'rawvideo', - '-vcodec', 'rawvideo', - '-s', f'{self.width}x{self.height}', - '-pix_fmt', 'bgr24', - '-r', str(self.fps), - '-i', '-', # Input from pipe - '-c:v', self.codec, - '-preset', 'p1', # Low latency preset - '-tune', 'ull', # Ultra Low Latency - '-zerolatency', '1', - '-g', '30', # Keyframe every 1s - '-bf', '0', # No B-frames - '-f', self.stream_fmt, - '-' # Output to pipe - ] + return True - try: - self.process = subprocess.Popen( - self.ffmpeg_cmd, - stdin=subprocess.PIPE, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - bufsize=0 # Unbuffered IO - ) - except FileNotFoundError: - self.get_logger().error("FFmpeg not found. Ensure ffmpeg is installed.") - return - - self.publisher_ = self.create_publisher(CompressedImage, self.topic_name, 10) - - # 3. Start Threads - self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) - - self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) - self.output_thread.start() - - self.get_logger().info(f"Streaming {self.width}x{self.height} via {self.codec} (MJPG Input)...") + def setup_ffmpeg(self): + """Initializes the FFmpeg subprocess.""" + if self.process: + self.process.kill() - def initalize_parameters(self): - # Define the parameter descriptor to specify the type - topic_parameter_descriptor = ParameterDescriptor( - type=ParameterType.PARAMETER_STRING, - description='A wecam topic to publish to.' - ) + ffmpeg_cmd = [ + 'ffmpeg', '-y', '-hide_banner', '-loglevel', 'error', + '-f', 'rawvideo', '-vcodec', 'rawvideo', + '-s', f'{self.width}x{self.height}', '-pix_fmt', 'bgr24', + '-r', str(self.fps), '-i', '-', + '-c:v', self.codec, '-preset', 'p1', '-tune', 'ull', + '-zerolatency', '1', '-g', '30', '-bf', '0', + '-f', self.stream_fmt, '-' + ] - camera_id_parameter_descriptor = ParameterDescriptor( - type=ParameterType.PARAMETER_STRING, - description='A wecam id to read from.' + self.process = subprocess.Popen( + ffmpeg_cmd, stdin=subprocess.PIPE, + stdout=subprocess.PIPE, stderr=subprocess.PIPE, bufsize=0 ) - self.declare_parameter( - 'topic_name', - value='', - descriptor=topic_parameter_descriptor - ) - - self.declare_parameter( - 'device_id', - value='0', - descriptor=camera_id_parameter_descriptor - ) - def feed_encoder(self): - """Capture frame and push to FFmpeg stdin""" - if not self.cap.isOpened(): + """Capture frame and push to FFmpeg. Handles reconnection if camera is lost.""" + if self.cap is None or not self.cap.isOpened(): + self.get_logger().warn(f"Camera {self.camera_source} non-existent. Retrying...", throttle_duration_sec=2.0) + if self.open_camera(): + self.setup_ffmpeg() + self.get_logger().info(f"Camera {self.camera_source} reconnected!") return - + ret, frame = self.cap.read() - if ret: - # Resize if the camera ignores our resolution request - if frame.shape[1] != self.width or frame.shape[0] != self.height: - frame = cv2.resize(frame, (self.width, self.height)) + + if not ret: + self.get_logger().error("Camera unplugged or read failed.") + self.cap.release() + return + + # Ensure correct dimensions for FFmpeg + if frame.shape[1] != self.width or frame.shape[0] != self.height: + frame = cv2.resize(frame, (self.width, self.height)) - try: + try: + if self.process and self.process.stdin: self.process.stdin.write(frame.tobytes()) self.process.stdin.flush() - except BrokenPipeError: - self.get_logger().error("FFmpeg stdin broken pipe") - rclpy.shutdown() - else: - self.get_logger().warn("Camera frame read failed.") + except (BrokenPipeError, AttributeError): + self.get_logger().error("FFmpeg pipe broken. Resetting...") + self.setup_ffmpeg() def read_encoded_stream(self): - """Continuously read from FFmpeg stdout and publish""" - # Buffer size large enough for 1080p frames to prevent artifacting + """Continuously read from FFmpeg stdout and publish.""" chunk_size = 256 * 1024 - - while rclpy.ok() and self.process.poll() is None: - try: - # Read whatever is available - data = self.process.stdout.read(chunk_size) - - if data: - msg = CompressedImage() - msg.header.stamp = self.get_clock().now().to_msg() - msg.format = self.stream_fmt - msg.data = data - - self.publisher_.publish(msg) - else: - time.sleep(0.002) - except Exception as e: - self.get_logger().error(f"Read error: {e}") - break + while rclpy.ok() and self.is_running: + if self.process and self.process.poll() is None: + try: + data = self.process.stdout.read(chunk_size) + if data: + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.format = self.stream_fmt + msg.data = data + self.publisher_.publish(msg) + else: + time.sleep(0.005) + except Exception: + time.sleep(0.1) + else: + time.sleep(0.1) + + def initalize_parameters(self): + self.declare_parameter('topic_name', value='', + descriptor=ParameterDescriptor(type=ParameterType.PARAMETER_STRING)) + self.declare_parameter('device_id', value='0', + descriptor=ParameterDescriptor(type=ParameterType.PARAMETER_STRING)) def destroy_node(self): + self.is_running = False if self.process: self.process.kill() - self.cap.release() + if self.cap: + self.cap.release() super().destroy_node() def main(args=None): @@ -186,4 +158,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/src/camera_interface/camera_interface/qt_user_interface.py b/src/camera_interface/camera_interface/qt_user_interface.py index d224148d..b936a4ef 100644 --- a/src/camera_interface/camera_interface/qt_user_interface.py +++ b/src/camera_interface/camera_interface/qt_user_interface.py @@ -142,13 +142,13 @@ class CameraWidget(QWidget): def __init__(self, topic): super().__init__() self.topic = topic - self.last_frame = None + self.last_frame = None # Stores the actual numpy array + self.last_received_time = time.time() + self.is_disconnected = True self.label = QLabel("Waiting for Stream...") self.label.setAlignment(Qt.AlignCenter) self.label.setStyleSheet("background-color: #000; color: #555;") - - # FIX 1: Use QSizePolicy instead of QWidget.SizePolicy self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) title = QLabel(topic) @@ -161,12 +161,62 @@ def __init__(self, topic): layout.addWidget(self.label, 1) layout.addWidget(title) + from PySide6.QtCore import QTimer + self.watchdog = QTimer(self) + self.watchdog.timeout.connect(self.check_connection) + self.watchdog.start(500) + + def check_connection(self): + """Checks if the frame rate has stalled.""" + if time.time() - self.last_received_time > 2.0: + # Re-draw the "No Signal" overlay even if already disconnected + # to handle window resizing properly. + self.show_no_signal() + else: + self.is_disconnected = False + + def show_no_signal(self): + """Overlays 'No Signal' on top of the last known frame.""" + self.is_disconnected = True + + # Create base pixmap (either the last frame or black) + if self.last_frame is not None: + h, w, ch = self.last_frame.shape + bytes_per_line = ch * w + qimg = QImage(self.last_frame.data, w, h, bytes_per_line, QImage.Format_RGB888) + base_pixmap = QPixmap.fromImage(qimg).scaled( + self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation + ) + else: + base_pixmap = QPixmap(self.label.size()) + base_pixmap.fill(Qt.black) + + # Create overlay + from PySide6.QtGui import QPainter, QColor, QFont + painter = QPainter(base_pixmap) + + # 1. Draw a semi-transparent dark rectangle over the whole frame + painter.fillRect(base_pixmap.rect(), QColor(0, 0, 0, 127)) + + # 2. Draw the text + painter.setPen(QColor(255, 50, 50)) # Bright Red + font = QFont("Arial", 16, QFont.Bold) + painter.setFont(font) + + painter.drawText(base_pixmap.rect(), Qt.AlignCenter, "NO SIGNAL") + painter.end() + + self.label.setPixmap(base_pixmap) + def update_image(self, cv_img): - self.last_frame = cv_img - h, w, ch = cv_img.shape - bytes_per_line = ch * w + """Updates the stored frame and resets the watchdog.""" + self.last_received_time = time.time() + self.is_disconnected = False + self.last_frame = cv_img.copy() # Store copy to prevent memory corruption - qimg = QImage(cv_img.data, w, h, bytes_per_line, QImage.Format_RGB888).copy() + h, w, ch = self.last_frame.shape + bytes_per_line = ch * w + qimg = QImage(self.last_frame.data, w, h, bytes_per_line, QImage.Format_RGB888) pix = QPixmap.fromImage(qimg) self.label.setPixmap(pix.scaled( @@ -174,10 +224,15 @@ def update_image(self, cv_img): )) def resizeEvent(self, event): - if self.last_frame is not None: + # Trigger redraw on resize so aspect ratio and overlay remain correct + if not self.is_disconnected and self.last_frame is not None: self.update_image(self.last_frame) + else: + self.show_no_signal() super().resizeEvent(event) +# -------------------- Updated UI Window logic -------------------- + class MultiCameraWindow(QMainWindow): def __init__(self, ros_node): super().__init__() @@ -248,10 +303,11 @@ def add_camera(self, topic, msg_type): if topic in self.camera_widgets: return widget = CameraWidget(topic) + # Using a lambda to ensure the widget receives the frame sig = ImageSignal() sig.image_ready.connect(widget.update_image) - self.ros_node.image_signals[topic] = sig + self.ros_node.image_signals[topic] = sig self.ros_node.subscribe(topic, msg_type) self.camera_widgets[topic] = widget self.rebuild_grid() diff --git a/src/camera_interface/launch/camera_system_launch.py b/src/camera_interface/launch/camera_system_launch.py index 386e456a..1f35afe0 100644 --- a/src/camera_interface/launch/camera_system_launch.py +++ b/src/camera_interface/launch/camera_system_launch.py @@ -7,11 +7,11 @@ def generate_launch_description(): # (e.g.) -> /dev/v4l/by-id/usb-Logitech_Webcam_C920_ABC123-video-index0 camera_configs = [ {"name": "right", "topic": "/webcam/right", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index0"}, - {"name": "left", "topic": "/webcam/left", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index0"}, + {"name": "left", "topic": "/webcam/left", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index0"}, # {"name": "back", "topic": "/webcam/back", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index0"}, {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index1"}, - {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/video4"}, - {"name": "front", "topic": "/webcam/front", "id": "/dev/video5"}, + {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index1"}, + {"name": "front", "topic": "/webcam/front", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index1"}, ] nodes = [ From 1b1d0f23613001cd6d1cc64c227387e83ad44165 Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Thu, 5 Mar 2026 19:02:00 -0600 Subject: [PATCH 12/21] Add qt interface ui changes to help the user see lost signals and to close the program when closing it in the terminal --- .../camera_interface/qt_user_interface.py | 83 ++++++++++++------- 1 file changed, 54 insertions(+), 29 deletions(-) diff --git a/src/camera_interface/camera_interface/qt_user_interface.py b/src/camera_interface/camera_interface/qt_user_interface.py index b936a4ef..887e0d96 100644 --- a/src/camera_interface/camera_interface/qt_user_interface.py +++ b/src/camera_interface/camera_interface/qt_user_interface.py @@ -2,7 +2,7 @@ import threading import time import math -import numpy as np +import signal import av # PyAV import rclpy @@ -17,7 +17,7 @@ QListWidget, QListWidgetItem, QGridLayout, QSizePolicy # <--- Added QSizePolicy import ) -from PySide6.QtGui import QImage, QPixmap +from PySide6.QtGui import QImage, QPixmap, QPainter, QColor, QFont from PySide6.QtCore import Qt, Signal, QObject # -------------------- Dark Theme -------------------- @@ -142,11 +142,12 @@ class CameraWidget(QWidget): def __init__(self, topic): super().__init__() self.topic = topic - self.last_frame = None # Stores the actual numpy array + self.last_frame = None self.last_received_time = time.time() + self.start_time = time.time() # Track when we started waiting self.is_disconnected = True - self.label = QLabel("Waiting for Stream...") + self.label = QLabel("Initializing...") self.label.setAlignment(Qt.AlignCenter) self.label.setStyleSheet("background-color: #000; color: #555;") self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) @@ -165,54 +166,64 @@ def __init__(self, topic): self.watchdog = QTimer(self) self.watchdog.timeout.connect(self.check_connection) self.watchdog.start(500) + + self.show_no_signal() def check_connection(self): """Checks if the frame rate has stalled.""" + # Check if we are timed out (either from a stall or initial wait) if time.time() - self.last_received_time > 2.0: - # Re-draw the "No Signal" overlay even if already disconnected - # to handle window resizing properly. self.show_no_signal() else: self.is_disconnected = False def show_no_signal(self): - """Overlays 'No Signal' on top of the last known frame.""" + """Displays status overlay based on connection history and elapsed time.""" self.is_disconnected = True + elapsed_since_start = time.time() - self.start_time + + base_pixmap = QPixmap(self.label.size()) - # Create base pixmap (either the last frame or black) + # Decide which text and color to use if self.last_frame is not None: + # SCENARIO 1: We had a frame, but it stopped. h, w, ch = self.last_frame.shape bytes_per_line = ch * w qimg = QImage(self.last_frame.data, w, h, bytes_per_line, QImage.Format_RGB888) base_pixmap = QPixmap.fromImage(qimg).scaled( self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation ) + overlay_color = QColor(0, 0, 0, 140) + text_str = "NO SIGNAL" + text_color = QColor(255, 50, 50) # Red else: - base_pixmap = QPixmap(self.label.size()) + # SCENARIO 2: We have never received a frame. base_pixmap.fill(Qt.black) + overlay_color = QColor(0, 0, 0, 0) + + # If we've been waiting more than 10 seconds, call it 'No Signal' + if elapsed_since_start > 10.0: + text_str = "NO SIGNAL (TIMEOUT)" + text_color = QColor(255, 50, 50) # Switch to Red + else: + text_str = "WAITING FOR CAMERA..." + text_color = QColor(180, 180, 180) # Neutral Grey - # Create overlay - from PySide6.QtGui import QPainter, QColor, QFont painter = QPainter(base_pixmap) + if overlay_color.alpha() > 0: + painter.fillRect(base_pixmap.rect(), overlay_color) - # 1. Draw a semi-transparent dark rectangle over the whole frame - painter.fillRect(base_pixmap.rect(), QColor(0, 0, 0, 127)) - - # 2. Draw the text - painter.setPen(QColor(255, 50, 50)) # Bright Red - font = QFont("Arial", 16, QFont.Bold) - painter.setFont(font) - - painter.drawText(base_pixmap.rect(), Qt.AlignCenter, "NO SIGNAL") + painter.setPen(text_color) + painter.setFont(QFont("Arial", 14, QFont.Bold)) + painter.drawText(base_pixmap.rect(), Qt.AlignCenter, text_str) painter.end() self.label.setPixmap(base_pixmap) def update_image(self, cv_img): - """Updates the stored frame and resets the watchdog.""" self.last_received_time = time.time() self.is_disconnected = False - self.last_frame = cv_img.copy() # Store copy to prevent memory corruption + self.last_frame = cv_img.copy() h, w, ch = self.last_frame.shape bytes_per_line = ch * w @@ -224,13 +235,12 @@ def update_image(self, cv_img): )) def resizeEvent(self, event): - # Trigger redraw on resize so aspect ratio and overlay remain correct - if not self.is_disconnected and self.last_frame is not None: - self.update_image(self.last_frame) - else: + if self.is_disconnected: self.show_no_signal() + elif self.last_frame is not None: + self.update_image(self.last_frame) super().resizeEvent(event) - + # -------------------- Updated UI Window logic -------------------- class MultiCameraWindow(QMainWindow): @@ -345,24 +355,39 @@ def main(): rclpy.init() node = UserInterfaceNode() + # Ensure this thread is a 'daemon' so it dies when the main process dies ros_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) ros_thread.start() app = QApplication(sys.argv) app.setStyleSheet(dark_stylesheet) + # --- SIGINT Handling --- + # This allows Ctrl+C in terminal to close the Qt Window + signal.signal(signal.SIGINT, lambda *args: QApplication.quit()) + + # A tiny timer that runs every 500ms just to give the + # Python interpreter a chance to catch the signal + from PySide6.QtCore import QTimer + timer = QTimer() + timer.start(500) + timer.timeout.connect(lambda: None) + # ----------------------- + gui = MultiCameraWindow(node) gui.resize(1280, 720) gui.show() try: - # FIX 2: Do not wrap in sys.exit() yet to allow clean shutdown block app.exec() + except KeyboardInterrupt: + pass finally: - # FIX 3: Check if rclpy is still okay before shutting down + # Clean shutdown node.destroy_node() if rclpy.ok(): rclpy.shutdown() + print("\nUI and ROS Node shut down successfully.") if __name__ == "__main__": main() \ No newline at end of file From fb98953f6c7e47bf977e46478a3ff11bb9c644fa Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Thu, 5 Mar 2026 19:50:59 -0600 Subject: [PATCH 13/21] Add v4l cameras and the /dev/videos to allow the symbolic links to find these to use --- scripts/enter_isaac_ros_container.sh | 14 +++++++++++++- .../launch/camera_system_launch.py | 6 +++--- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/scripts/enter_isaac_ros_container.sh b/scripts/enter_isaac_ros_container.sh index 03185f19..bfdc20f0 100755 --- a/scripts/enter_isaac_ros_container.sh +++ b/scripts/enter_isaac_ros_container.sh @@ -1,7 +1,19 @@ #!/bin/bash printf 'CONFIG_DOCKER_SEARCH_DIRS="$HOME/Lunabotics/src/isaac_ros/isaac_ros_common/scripts/../../../../docker"\n BASE_DOCKER_REGISTRY_NAMES="umnrobotics/isaac_ros3.1"\n'> ~/.isaac_ros_common-config image_key="ros2_humble.deepstream.user.zed.umn.gazebo" -docker_arg="-v /usr/local/zed/resources:/usr/local/zed/resources -v $HOME/rosbags:/rosbags -v /usr/local/zed/settings:/usr/local/zed/settings" + +# Start with your existing volumes +docker_arg="-v /usr/local/zed/resources:/usr/local/zed/resources \ +-v $HOME/rosbags:/rosbags \ +-v /usr/local/zed/settings:/usr/local/zed/settings \ +-v /dev/v4l:/dev/v4l" + +# Dynamically add ALL video devices present on the host +for dev in /dev/video*; do + if [ -e "$dev" ]; then + docker_arg="$docker_arg --device $dev:$dev" + fi +done USE_CACHED_IMAGE=${1:-true} diff --git a/src/camera_interface/launch/camera_system_launch.py b/src/camera_interface/launch/camera_system_launch.py index 1f35afe0..ca34797d 100644 --- a/src/camera_interface/launch/camera_system_launch.py +++ b/src/camera_interface/launch/camera_system_launch.py @@ -9,9 +9,9 @@ def generate_launch_description(): {"name": "right", "topic": "/webcam/right", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index0"}, {"name": "left", "topic": "/webcam/left", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index0"}, # {"name": "back", "topic": "/webcam/back", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index0"}, - {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index1"}, - {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index1"}, - {"name": "front", "topic": "/webcam/front", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index1"}, + # {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index1"}, + # {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index1"}, + # {"name": "front", "topic": "/webcam/front", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index1"}, ] nodes = [ From 8e722f0baf6d027ee65dbe2c3d419cf79bc1d0ca Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Tue, 17 Mar 2026 18:43:21 -0500 Subject: [PATCH 14/21] Add /dev/ to the volume --- scripts/enter_isaac_ros_container.sh | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/scripts/enter_isaac_ros_container.sh b/scripts/enter_isaac_ros_container.sh index bfdc20f0..2deb71c7 100755 --- a/scripts/enter_isaac_ros_container.sh +++ b/scripts/enter_isaac_ros_container.sh @@ -6,14 +6,8 @@ image_key="ros2_humble.deepstream.user.zed.umn.gazebo" docker_arg="-v /usr/local/zed/resources:/usr/local/zed/resources \ -v $HOME/rosbags:/rosbags \ -v /usr/local/zed/settings:/usr/local/zed/settings \ --v /dev/v4l:/dev/v4l" - -# Dynamically add ALL video devices present on the host -for dev in /dev/video*; do - if [ -e "$dev" ]; then - docker_arg="$docker_arg --device $dev:$dev" - fi -done +-v /dev:/dev \ +--privileged" USE_CACHED_IMAGE=${1:-true} From 807fbd9f3fac89615b83ad0dcce1299157cdcd25 Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Tue, 17 Mar 2026 19:29:45 -0500 Subject: [PATCH 15/21] Add manual video file mounts to ros container launch --- config/auger_config.yaml | 6 +++--- scripts/enter_isaac_ros_container.sh | 12 ++++++++++-- src/camera_interface/launch/camera_system_launch.py | 6 +++--- src/rovr_control/package.xml | 2 +- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/config/auger_config.yaml b/config/auger_config.yaml index 5d2a7153..3572ad4e 100644 --- a/config/auger_config.yaml +++ b/config/auger_config.yaml @@ -9,13 +9,13 @@ MAX_EXTEND_PUSH_MOTOR_VELOCITY: 600 # in RPM push_motor_position: 0 tilt_actuator_position: 0 - TILT_ACTUATOR_CURRENT_THRESHOLD: 0 + TILT_ACTUATOR_CURRENT_THRESHOLD: 99999 MAX_PUSH_MOTOR_POSITION: 0 MIN_PUSH_MOTOR_POSITION: 0 PUSH_MOTOR_POS_TOLERANCE: 0 MAX_PUSH_MOTOR_VELOCITY: 0 - TILT_ACTUATOR_SPEED: 0 + TILT_ACTUATOR_SPEED: 25 TILT_ACTUATOR_MIN_EXTENSION: 0 - TILT_ACTUATOR_ID: 0 + TILT_ACTUATOR_ID: 7 PUSH_MOTOR_ID: 0 SPIN_MOTOR_ID: 0 \ No newline at end of file diff --git a/scripts/enter_isaac_ros_container.sh b/scripts/enter_isaac_ros_container.sh index 2deb71c7..ceaa73f2 100755 --- a/scripts/enter_isaac_ros_container.sh +++ b/scripts/enter_isaac_ros_container.sh @@ -6,8 +6,16 @@ image_key="ros2_humble.deepstream.user.zed.umn.gazebo" docker_arg="-v /usr/local/zed/resources:/usr/local/zed/resources \ -v $HOME/rosbags:/rosbags \ -v /usr/local/zed/settings:/usr/local/zed/settings \ --v /dev:/dev \ ---privileged" +-v /dev/v4l:/dev/v4l \ +-v /dev/video0:/dev/video0 \ +-v /dev/video1:/dev/video1 \ +-v /dev/video2:/dev/video2 \ +-v /dev/video3:/dev/video3 \ +-v /dev/video4:/dev/video4 \ +-v /dev/video5:/dev/video5 \ +-v /dev/video6:/dev/video6 \ +-v /dev/video7:/dev/video7 \ +-v /dev/video8:/dev/video8" USE_CACHED_IMAGE=${1:-true} diff --git a/src/camera_interface/launch/camera_system_launch.py b/src/camera_interface/launch/camera_system_launch.py index ca34797d..1f35afe0 100644 --- a/src/camera_interface/launch/camera_system_launch.py +++ b/src/camera_interface/launch/camera_system_launch.py @@ -9,9 +9,9 @@ def generate_launch_description(): {"name": "right", "topic": "/webcam/right", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index0"}, {"name": "left", "topic": "/webcam/left", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index0"}, # {"name": "back", "topic": "/webcam/back", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index0"}, - # {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index1"}, - # {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index1"}, - # {"name": "front", "topic": "/webcam/front", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index1"}, + {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index1"}, + {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index1"}, + {"name": "front", "topic": "/webcam/front", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index1"}, ] nodes = [ diff --git a/src/rovr_control/package.xml b/src/rovr_control/package.xml index cdb94d41..7adee727 100644 --- a/src/rovr_control/package.xml +++ b/src/rovr_control/package.xml @@ -26,7 +26,7 @@ ros2launch motor_control drivetrain - digger + auger dumper joy rovr_interfaces From 3edaca602057facdb26b8203be8e0367f49c9adb Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Tue, 21 Apr 2026 19:17:27 -0500 Subject: [PATCH 16/21] Testing cameras --- src/camera_interface/launch/camera_system_launch.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/camera_interface/launch/camera_system_launch.py b/src/camera_interface/launch/camera_system_launch.py index 1f35afe0..2b8eaa72 100644 --- a/src/camera_interface/launch/camera_system_launch.py +++ b/src/camera_interface/launch/camera_system_launch.py @@ -6,12 +6,13 @@ def generate_launch_description(): # To get the list of ids, we do : ls -l /dev/v4l/by-id/ # (e.g.) -> /dev/v4l/by-id/usb-Logitech_Webcam_C920_ABC123-video-index0 camera_configs = [ - {"name": "right", "topic": "/webcam/right", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index0"}, - {"name": "left", "topic": "/webcam/left", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index0"}, # - {"name": "back", "topic": "/webcam/back", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index0"}, - {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index1"}, - {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index1"}, - {"name": "front", "topic": "/webcam/front", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index1"}, + {"name": "right", "topic": "/webcam/right", "id": "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB2.0_FHD_UVC_WebCam-video-index0"}, + {"name": "left", "topic": "/webcam/left", "id": "/dev/v4l/by-id/usb-OmniVision_Technologies__Inc._2000-video-index0"}, # + {"name": "back", "topic": "/webcam/back", "id": "/dev/v4l/by-id/usb-OmniVision_Technologies__Inc._USB_Camera-B4.09.24.1-video-index0"}, + {"name": "digger", "topic": "/webcam/digger", "id": "/dev/v4l/by-id/usb-Technologies__Inc._ZED_2i_OV0001-video-index0"}, + {"name": "digger", "topic": "/webcam/digger_2", "id": "/dev/v4l/by-id/usb-Technologies__Inc._ZED_2i_OV0001-video-index1"}, + # {"name": "dumper", "topic": "/webcam/dumper", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9281_USB_Camera_UC762-video-index1"}, + # {"name": "front", "topic": "/webcam/front", "id": "/dev/v4l/by-id/usb-Arducam_Technology_Co.__Ltd._Arducam_OV9782_USB_Camera_UC852-video-index1"}, ] nodes = [ From d06e5e125c512f1300104fefb72b9fae36771ef7 Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Sun, 26 Apr 2026 17:44:53 -0500 Subject: [PATCH 17/21] testing changes from past week --- arduino/sensors/sensors.ino | 18 +- config/auger_config.yaml | 22 +- config/dumper_config.yaml | 2 +- config/motor_control.yaml | 11 +- src/auger/auger/auger_node.py | 326 ++++++++++++------ src/drivetrain/drivetrain/drivetrain_node.py | 129 +++++-- src/dumper/dumper/dumper_node.py | 119 +++++-- .../rovr_control/auto_dig_server.py | 14 +- .../rovr_control/main_control_node.py | 10 - src/rovr_control/rovr_control/read_serial.py | 24 +- .../rovr_control/xbox_controller_constants.py | 2 +- 11 files changed, 460 insertions(+), 217 deletions(-) diff --git a/arduino/sensors/sensors.ino b/arduino/sensors/sensors.ino index dc9ac952..ffab8bd2 100644 --- a/arduino/sensors/sensors.ino +++ b/arduino/sensors/sensors.ino @@ -1,13 +1,12 @@ // Define a struct to hold the sensor data struct SensorData { - int leftMotorPotentiometer; - int rightMotorPotentiometer; - bool bottomLimitSwitch; + bool dumpLimitSwitch; + bool extensionLimitSwitch; }; // Define the sensor pins here -#define LEFT_MOTOR_POT_PIN A0 -#define RIGHT_MOTOR_POT_PIN A1 +#define DUMP_LIMIT_SWITCH 12 +#define EXTENSION_LIMIT_SWITCH 13 #define RELAY_PIN 7 void setup() { @@ -17,6 +16,9 @@ void setup() { pinMode(RELAY_PIN, OUTPUT); // Set relay pin as an output digitalWrite(RELAY_PIN, LOW); // Ensure the motor is off at the start + pinMode(DUMP_LIMIT_SWITCH, INPUT_PULLUP); + pinMode(EXTENSION_LIMIT_SWITCH, INPUT_PULLUP); + // No need to configure analog pins explicitly for potentiometers // as they are used as inputs by default. } @@ -26,10 +28,8 @@ void loop() { SensorData data; // Read from the analog inputs (potentiometers) - data.leftMotorPotentiometer = analogRead(LEFT_MOTOR_POT_PIN); // Read left motor potentiometer value - data.rightMotorPotentiometer = analogRead(RIGHT_MOTOR_POT_PIN); // Read right motor potentiometer value - - data.bottomLimitSwitch = analogRead(bottom_limit_switch); //bottom limit switch value + data.dumpLimitSwitch = digitalRead(DUMP_LIMIT_SWITCH); + data.extensionLimitSwitch = digitalRead(EXTENSION_LIMIT_SWITCH); // Send the struct over the serial bus to the Nvidia Jetson Serial.write((byte *)&data, sizeof(SensorData)); diff --git a/config/auger_config.yaml b/config/auger_config.yaml index 3572ad4e..99067836 100644 --- a/config/auger_config.yaml +++ b/config/auger_config.yaml @@ -3,19 +3,15 @@ #TODO: Figure out all of these values SCREW_SPEED: 4000 # TODO: update POWER_LIMIT: 1 #TODO: update - MAX_SCREW_SPEED: 4000 # in RPM - MIN_SCREW_SPEED: 2000 # in RPM TODO: This is a guess - MAX_RETRACT_PUSH_MOTOR_VELOCITY: -600 # in RPM, potentially could be faster - MAX_EXTEND_PUSH_MOTOR_VELOCITY: 600 # in RPM - push_motor_position: 0 - tilt_actuator_position: 0 - TILT_ACTUATOR_CURRENT_THRESHOLD: 99999 - MAX_PUSH_MOTOR_POSITION: 0 - MIN_PUSH_MOTOR_POSITION: 0 + FAST_SCREW_SPEED:LD: 0.0 #0.21 + extension_limit_switch: true + STOWED: true + MAX_PUSH_MOTOR_POSITION: 10000 + MIN_PUSH_MOTOR_POSITION: 1000 PUSH_MOTOR_POS_TOLERANCE: 0 MAX_PUSH_MOTOR_VELOCITY: 0 - TILT_ACTUATOR_SPEED: 25 + TILT_ACTUATOR_SPEED: 1.0 TILT_ACTUATOR_MIN_EXTENSION: 0 - TILT_ACTUATOR_ID: 7 - PUSH_MOTOR_ID: 0 - SPIN_MOTOR_ID: 0 \ No newline at end of file + TILT_ACTUATOR_ID: 1 + PUSH_MOTOR_ID: 3 + SPIN_MOTOR_ID: 4 \ No newline at end of file diff --git a/config/dumper_config.yaml b/config/dumper_config.yaml index 7013fd33..b300a393 100644 --- a/config/dumper_config.yaml +++ b/config/dumper_config.yaml @@ -1,3 +1,3 @@ /**: ros__parameters: - DUMPER_POWER: 0.75 # The power the dumper needs to go + DUMPER_POWER: -0.75 # The power the dumper needs to go diff --git a/config/motor_control.yaml b/config/motor_control.yaml index a756c74c..599ee9d4 100644 --- a/config/motor_control.yaml +++ b/config/motor_control.yaml @@ -2,10 +2,11 @@ ros__parameters: CAN_INTERFACE_TRANSMIT: "can0" # can0, can1, slcan0, slcan1 are common values CAN_INTERFACE_RECEIVE: "can0" # can0, can1, slcan0, slcan1 are common values - BACK_LEFT_DRIVE: 5 # CAN ID of the back left drive motor - FRONT_LEFT_DRIVE: 3 # CAN ID of the front left drive motor - BACK_RIGHT_DRIVE: 1 # CAN ID of the back right drive motor - FRONT_RIGHT_DRIVE: 6 # CAN ID of the front right drive motor + CAN_INTERFACE: "can0" # can0, can1, slcan0, slcan1 are common values + BACK_LEFT_DRIVE: 6 # CAN ID of the back left drive motor + FRONT_LEFT_DRIVE: 8 # CAN ID of the front left drive motor + BACK_RIGHT_DRIVE: 7 # CAN ID of the back right drive motor + FRONT_RIGHT_DRIVE: 5 # CAN ID of the front right drive motor DIGGER_MOTOR: 10 # CAN ID of the digger chain motor DIGGER_LEFT_LINEAR_ACTUATOR: 8 # CAN ID of the left linear actuator DIGGER_RIGHT_LINEAR_ACTUATOR: 7 # CAN ID of the right linear actuator @@ -16,7 +17,7 @@ DIGGER_ACTUATORS_kP_coupling: 0.004 # kP for digger actuator position syncing DIGGER_PITCH_kP: 2.5 # kP for tipping while digging TIPPING_SPEED_ADJUSTMENT: false # determines if pitch speed adjustment is used - DIGGER_SAFETY_ZONE: 120 # Measured in potentiometer units (0 to 1023) + EXTENSION_SAFETY_ZONE: 0 # Measured in potentiometer units (0 to 1023) CURRENT_SPIKE_THRESHOLD: 1.8 # Threshold for current spike detection (measured in Amps) CURRENT_SPIKE_TIME: 0.2 # Time in seconds to consider it a current spike (measured in seconds) BUCKETS_CURRENT_SPIKE_THRESHOLD: 12.0 # Threshold for current spike detection (measured in Amps) diff --git a/src/auger/auger/auger_node.py b/src/auger/auger/auger_node.py index d91f98c2..fb49601b 100644 --- a/src/auger/auger/auger_node.py +++ b/src/auger/auger/auger_node.py @@ -18,8 +18,8 @@ SetScrewMotorSpeed, ) from rovr_interfaces.srv import SetExtension -from rovr_interfaces.msg import Potentiometers from std_srvs.srv import Trigger +from std_msgs.msg import Bool, Float32 class Auger(Node): @@ -37,30 +37,32 @@ def __init__(self): self.cli_motor_get = self.create_client(MotorCommandGet, "motor/get") # Define parameters here + self.declare_parameter("AUGER_STOWED", True) + self.declare_parameter("DUMPER_STOWED", True) + self.declare_parameter("extension_limit_switch", True) self.declare_parameter("POWER_LIMIT", 1) self.declare_parameter("SCREW_SPEED", 4000) self.declare_parameter( "MAX_SCREW_SPEED", 4_000 ) # in RPM for both negative and positive direction - self.declare_parameter("MIN_SCREW_DIG_SPEED", 2_000) + self.declare_parameter("MIN_SCREW_DIG_SPEED", 2000) self.declare_parameter("MAX_SPIN_MOTOR_CURRENT", 0) self.declare_parameter("push_motor_position", 0) - self.declare_parameter("tilt_actuator_position", 0) # The error range to consider the current 0 - self.declare_parameter("TILT_ACTUATOR_CURRENT_THRESHOLD", 0) + self.declare_parameter("TILT_ACTUATOR_CURRENT_THRESHOLD", 0.21) # TODO: Find real value for this - self.declare_parameter("MAX_PUSH_MOTOR_POSITION", 0) - self.declare_parameter("MIN_PUSH_MOTOR_POSITION", 0) - self.declare_parameter("DEFAULT_PUSH_MOTOR_SPEED", 0) + self.declare_parameter("MAX_PUSH_MOTOR_POSITION", 10000) + self.declare_parameter("MIN_PUSH_MOTOR_POSITION", 1000) + self.declare_parameter("DEFAULT_PUSH_MOTOR_SPEED", 3000) self.declare_parameter("MAX_PUSH_MOTOR_CURRENT", 0) self.declare_parameter("PUSH_MOTOR_POS_TOLERANCE", 0) # Could potentially be faster self.declare_parameter("MAX_RETRACT_PUSH_MOTOR_VELOCITY", -600) self.declare_parameter("MAX_EXTEND_PUSH_MOTOR_VELOCITY", 600) # Since we only care if the tilt actuator is fully extened or fully - # retracted then we should only need to care about the speed it moves + #m retracted then we should only need to care about the speed it moves # make sure to verify direction of this velocity - self.declare_parameter("TILT_ACTUATOR_SPEED", 0) + self.declare_parameter("TILT_ACTUATOR_SPEED", 1.0) # Minimum amount the tilt actuator needs to be extened to safely extend # push motor self.declare_parameter("TILT_ACTUATOR_MIN_EXTENSION", 0) @@ -68,21 +70,25 @@ def __init__(self): # tilt actuator self.declare_parameter("PUSH_MOTOR_MIN_RETRACTION", 0) # TODO Get real can ids - self.declare_parameter("TILT_ACTUATOR_ID", 0) + self.declare_parameter("TILT_ACTUATOR_ID", 1) self.declare_parameter("PUSH_MOTOR_ID", 0) self.declare_parameter("SPIN_MOTOR_ID", 0) + self.declare_parameter("FAST_SCREW_SPEED", 3000) + self.declare_parameter("SLOW_SCREW_SPEED", 0) # Local variables here - self.POWER_LIMIT = self.get_parameter("POWER_LIMIT").value - self.SCREW_SPEED = self.get_parameter("SCREW_SPEED").value - self.MAX_SCREW_SPEED = self.get_parameter("MAX_SCREW_SPEED").value self.MIN_SCREW_DIG_SPEED = self.get_parameter("MIN_SCREW_DIG_SPEED").value + self.POWER_LIMIT = self.get_parameter("POWER_LIMIT").value + self.FAST_SCREW_SPEED = self.get_parameter("FAST_SCREW_SPEED").value + self.SLOW_SCREW_SPEED = self.get_parameter("SLOW_SCREW_SPEED").value self.MAX_SPIN_MOTOR_CURRENT = self.get_parameter("MAX_SPIN_MOTOR_CURRENT").value self.push_motor_position = self.get_parameter("push_motor_position").value - self.tilt_actuator_position = self.get_parameter("tilt_actuator_position").value + self.extension_limit_switch = self.get_parameter("extension_limit_switch").value + self.auger_stowed = self.get_parameter("AUGER_STOWED").value + self.dumper_stowed = self.get_parameter("DUMPER_STOWED").value self.TILT_ACTUATOR_CURRENT_THRESHOLD = self.get_parameter( "TILT_ACTUATOR_CURRENT_THRESHOLD" - ) + ).value self.MAX_PUSH_MOTOR_POSITION = self.get_parameter( "MAX_PUSH_MOTOR_POSITION" ).value @@ -113,6 +119,10 @@ def __init__(self): self.PUSH_MOTOR_ID = self.get_parameter("PUSH_MOTOR_ID").value self.SPIN_MOTOR_ID = self.get_parameter("SPIN_MOTOR_ID").value + self.linear_actuator_tilted: bool = False + self.cancel_linear_actuator: bool = False + self.linear_actuator_running: bool = False + # TODO Define services (methods callable from the outside) here self.srv_set_tilt_extension = self.create_service( SetExtension, @@ -128,12 +138,12 @@ def __init__(self): callback_group=self.service_cb_group, ) - self.srv_set_push_position = self.create_service( - AugerSetPushMotor, - "auger/push_motor/setPosition", - self.set_push_position_callback, - callback_group=self.service_cb_group, - ) + # self.srv_set_push_position = self.create_service( + # AugerSetPushMotor, + # "auger/push_motor/setPosition", + # self.set_push_position_callback, + # callback_group=self.service_cb_group, + # ) self.srv_stop_push = self.create_service( Trigger, @@ -184,20 +194,26 @@ def __init__(self): callback_group=self.service_cb_group, ) - self.srv_retract_digger = self.create_service( + self.srv_stop_all = self.create_service( Trigger, "auger/control/stop_all", - self.stop_all, + self.stop_all_callback, callback_group=self.service_cb_group, ) - # TODO Define subscribers here - need to subscribe to potentiometer - # readings? - self.potentiometer_sub = self.create_subscription( - Potentiometers, "potentiometer", self.push_motor_position_callback, 10 + + self.limit_switch_sub = self.create_subscription( + Bool, "ExtensionLimitSwitch", self.limit_switch_callback, 10 + ) + + self.dumper_stowed_sub = self.create_subscription( + Bool, "dumper_stowed", self.dumper_stowed_callback, 10 ) # TODO Define publishers here + self.auger_stowed_pub = self.create_publisher(Bool, "auger_stowed", 10) + + self.extension_pos_sub = self.create_publisher(Float32, "extension_pos", 10) # Define subsystem methods here @@ -208,21 +224,24 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: This will return false and do nothing if the push motor is currently extended. Caller is responsible for timeouts. """ - push_motor_pos_future = self.cli_motor_get.call_async( - MotorCommandGet.Request(type="position", can_id=self.PUSH_MOTOR_ID) - ) - rclpy.spin_until_future_complete(self, push_motor_pos_future) - push_motor_pos = push_motor_pos_future.result() - if not push_motor_pos.success: - self.get_logger().info( - "WARNING: Failed to move the tilt actuator because the push motor position could not be determined" - ) - return False - if push_motor_pos.data > self.PUSH_MOTOR_MIN_RETRACTION: - self.get_logger().info( - "WARNING: Failed to move the tilt actuator because the push motor is extended too far" - ) - return False + self.linear_actuator_running = True + # push_motor_pos_future = self.cli_motor_get.call_async( + # MotorCommandGet.Request(type="position", can_id=self.PUSH_MOTOR_ID) + # ) + # rclpy.spin_until_future_complete(self, push_motor_pos_future) + # push_motor_pos = push_motor_pos_future.result() + # if not push_motor_pos.success: + # self.get_logger().info( + # "WARNING: Failed to move the tilt actuator because the push motor position could not be determined" + # ) + # self.linear_actuator_running = False + # return False + # if push_motor_pos.data > self.PUSH_MOTOR_MIN_RETRACTION: + # self.get_logger().info( + # "WARNING: Failed to move the tilt actuator because the push motor is extended too far" + # ) + # self.linear_actuator_running = False + # return False if tilt: self.get_logger().info("Extending tilt actuator") @@ -233,16 +252,25 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: motor_set_future = self.cli_motor_set.call_async( MotorCommandSet.Request( - type="velocity", can_id=self.TILT_ACTUATOR_ID, value=speed + type="duty_cycle", can_id=self.TILT_ACTUATOR_ID, value=float(speed) ) ) + rclpy.spin_until_future_complete(self, motor_set_future) if not motor_set_future.result().success: self.get_logger().info("WARNING: Failed to set tilt motor velocity") + self.linear_actuator_running = False return False - + # gets motor current until it is 0 which means it has hit an limit # switch + time.sleep(1.5) + if tilt: + self.auger_stowed = False + msg = Bool() + msg.data = self.auger_stowed + self.auger_stowed_pub.publish(msg) + while True: motor_get_future = self.cli_motor_get.call_async( MotorCommandGet.Request( @@ -251,8 +279,8 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: ) ) rclpy.spin_until_future_complete(self, motor_get_future) - if motor_get_future.result().success: + self.get_logger().info(f"Actuator current: {motor_get_future.result().data}") if ( abs(motor_get_future.result().data) < self.TILT_ACTUATOR_CURRENT_THRESHOLD @@ -262,11 +290,37 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: self.get_logger().info("WARNING: Failed to read tilt actuator position") time.sleep(0.1) + + if self.cancel_linear_actuator: + break + + + motor_set_future_stop = self.cli_motor_set.call_async( + MotorCommandSet.Request( + type="duty_cycle", can_id=self.TILT_ACTUATOR_ID, value=0.0 + ) + ) + rclpy.spin_until_future_complete(self, motor_set_future_stop) + self.linear_actuator_running = False + if not motor_set_future_stop.result().success: + self.get_logger().info("WARNING: Failed to stop tilt motor") + return False + if not tilt: + self.auger_stowed = True + msg = Bool() + msg.data = self.auger_stowed + self.auger_stowed_pub.publish(msg) + + self.linear_actuator_tilted = tilt return True def stop_actuator_tilt(self) -> bool: """Stop the auger angular position of the auger motor.""" + if self.linear_actuator_running: + self.cancel_linear_actuator = True + return True + self.get_logger().info("Stopping tilt actuator") motor_set_future = self.cli_motor_set.call_async( MotorCommandSet.Request( @@ -278,16 +332,15 @@ def stop_actuator_tilt(self) -> bool: rclpy.spin_until_future_complete(self, motor_set_future) return motor_set_future.result().success - def set_motor_push_position( - self, speed: float, desired_position: float, power_limit: float - ) -> bool: + def set_motor_push_extend(self) -> bool: """ Set the target position of the motor that pushes the auger into the ground. This will spin until the motor reaches given setpoint. This will fail if the screw is not spinning fast enough Caller is responsible for timeouts. """ - if self.tilt_actuator_position < self.TILT_ACTUATOR_MIN_EXTENSION: + power_limit = 0.5 + if not self.linear_actuator_tilted: self.get_logger().warn( "WARNING: Push motor will not move because the tilt actuator is not extended" ) @@ -313,18 +366,18 @@ def set_motor_push_position( return False if ( - desired_position > self.MAX_PUSH_MOTOR_POSITION - or desired_position < self.MIN_PUSH_MOTOR_POSITION + self.MAX_PUSH_MOTOR_POSITION > self.MAX_PUSH_MOTOR_POSITION + or self.MAX_PUSH_MOTOR_POSITION < self.MIN_PUSH_MOTOR_POSITION ): self.get_logger().warn( - f"WARNING: Requested push motor position is out of range, clamping value; requested: {desired_position}" + f"WARNING: Requested push motor position is out of range, clamping value; requested: {self.MAX_PUSH_MOTOR_POSITION}" ) - desired_position = max( + self.MAX_PUSH_MOTOR_POSITION = max( self.MIN_PUSH_MOTOR_POSITION, - min(desired_position, self.MAX_PUSH_MOTOR_POSITION), + min(self.MAX_PUSH_MOTOR_POSITION, self.MAX_PUSH_MOTOR_POSITION), ) # clamp the value to be within range self.get_logger().info( - "Setting auger push motor position to: " + str(desired_position) + "Setting auger push motor position to: " + str(self.MAX_PUSH_MOTOR_POSITION) ) motor_set_future = self.cli_motor_set.call_async( @@ -332,7 +385,7 @@ def set_motor_push_position( type="velocity", power_limit=power_limit, can_id=self.PUSH_MOTOR_ID, - value=float(speed), + value=float(self.DEFAULT_PUSH_MOTOR_SPEED), ) ) rclpy.spin_until_future_complete(self, motor_set_future) @@ -353,8 +406,12 @@ def set_motor_push_position( if motor_get_pos_future.result().success: current_pos = motor_get_pos_future.result().data - if (speed <= 0 and current_pos <= desired_position) or ( - speed > 0 and current_pos >= desired_position + msg = Float32() + msg.data = current_pos + self.extension_pos_sub.publish(msg) + if ( + (self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MAX_PUSH_MOTOR_POSITION) + or (self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MAX_PUSH_MOTOR_POSITION) ): break else: @@ -377,23 +434,84 @@ def set_motor_push_position( return True - def extend_motor_push(self) -> bool: + def set_motor_push_retract(self) -> bool: """ - Extends the push motor at max extend speed, returns false - if it is not able to due to the tilt actuator not being fully extended. + Set the target position of the motor that pushes the auger into the ground. + This will spin until the motor reaches given setpoint. + This will fail if the screw is not spinning fast enough + Caller is responsible for timeouts. """ - return self.set_motor_push_position( - self.DEFAULT_PUSH_MOTOR_SPEED, self.MAX_PUSH_MOTOR_POSITION, 0.5 + power_limit = 0.5 + + if ( + self.MIN_PUSH_MOTOR_POSITION > self.MIN_PUSH_MOTOR_POSITION + or self.MIN_PUSH_MOTOR_POSITION < self.MIN_PUSH_MOTOR_POSITION + ): + self.get_logger().warn( + f"WARNING: Requested push motor position is out of range, clamping value; requested: {self.MIN_PUSH_MOTOR_POSITION}" + ) + self.MIN_PUSH_MOTOR_POSITION = max( + self.MIN_PUSH_MOTOR_POSITION, + min(self.MIN_PUSH_MOTOR_POSITION, self.MIN_PUSH_MOTOR_POSITION), + ) # clamp the value to be within range + self.get_logger().info( + "Setting auger push motor position to: " + str(self.MIN_PUSH_MOTOR_POSITION) ) - def retract_motor_push(self) -> bool: - """ - Run the push motor at max retract speed. - Returns false if it is not able to due to the tilt actuator not being fully extended. - """ - return self.set_motor_push_position( - -self.DEFAULT_PUSH_MOTOR_SPEED, self.MIN_PUSH_MOTOR_POSITION, 0.5 + motor_set_future = self.cli_motor_set.call_async( + MotorCommandSet.Request( + type="velocity", + power_limit=power_limit, + can_id=self.PUSH_MOTOR_ID, + value=float(-self.DEFAULT_PUSH_MOTOR_SPEED), + ) + ) + rclpy.spin_until_future_complete(self, motor_set_future) + + if not motor_set_future.result().success: + self.get_logger().warn("WARNING: Failed to set push motor voltage") + return False + + # wait till motor reaches desired position + while True: + motor_get_pos_future = self.cli_motor_get.call_async( + MotorCommandGet.Request( + type="position", + can_id=self.PUSH_MOTOR_ID, + ) + ) + rclpy.spin_until_future_complete(self, motor_get_pos_future) + + if motor_get_pos_future.result().success: + current_pos = motor_get_pos_future.result().data + msg = Float32() + msg.data = current_pos + self.extension_pos_sub.publish(msg) + if ( + (-self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MIN_PUSH_MOTOR_POSITION) + or (-self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MIN_PUSH_MOTOR_POSITION) + or (self.extension_limit_switch) + ): + break + else: + self.get_logger().warn("WARNING: Failed to read push motor position") + + time.sleep(0.1) + + stop_motor_future = self.cli_motor_set.call_async( + MotorCommandSet.Request( + type="velocity", + power_limit=power_limit, + can_id=self.PUSH_MOTOR_ID, + value=0, + ) ) + stop_motor_response = rclpy.spin_until_future_complete(self, stop_motor_future) + if not stop_motor_response.result().success: + self.get_logger().warn("WARNING: Failed to stop the auger screw") + return False + + return True def stop_motor_push(self) -> bool: """Stop the motor that pushes the auger into the ground.""" @@ -409,12 +527,18 @@ def stop_motor_push(self) -> bool: def run_auger_spin_velocity(self, desired_speed: float, power_limit: float) -> bool: """Set the auger spin velocity of the auger motor.""" - if abs(desired_speed) > self.MAX_SCREW_SPEED: + if desired_speed < 0: + self.get_logger().info( + f"WARNING: Requested auger screw speed backwards, instead setting to 0" + ) + + desired_speed = 0 + elif desired_speed > self.FAST_SCREW_SPEED: self.get_logger().info( - f"WARNING: Requested auger screw speed is too fast: {desired_speed}" + f"WARNING: Requested auger screw speed is too fast, clamping it" ) - # This is the same as signum(desired_speed) * self.MAX_SCREW_SPEED - desired_speed = math.copysign(self.MAX_SCREW_SPEED, desired_speed) + desired_speed = self.FAST_SCREW_SPEED + self.get_logger().info(f"Running auger spin at velocity: {desired_speed}") @@ -423,7 +547,7 @@ def run_auger_spin_velocity(self, desired_speed: float, power_limit: float) -> b type="velocity", can_id=self.SPIN_MOTOR_ID, value=float(desired_speed), - power_limit=power_limit, + power_limit=float(power_limit), ) ) rclpy.spin_until_future_complete(self, motor_set_future) @@ -445,17 +569,19 @@ def stop_auger_spin(self) -> bool: def extend_digger(self) -> bool: """Tilt and extend""" + if not self.dumper_stowed: + return False tilt_success = self.set_actuator_tilt_extension(True) if not tilt_success: return False - spin_success = self.run_auger_spin_velocity(self.SCREW_SPEED, self.POWER_LIMIT) + spin_success = self.run_auger_spin_velocity(self.FAST_SCREW_SPEED, self.POWER_LIMIT) if not spin_success: return False - extend_success = self.extend_motor_push() + extend_success = self.set_motor_push_extend() if not extend_success: return False @@ -464,7 +590,7 @@ def extend_digger(self) -> bool: def retract_digger(self) -> bool: """Tilt and retract""" - retract_success = self.retract_motor_push() + retract_success = self.set_motor_push_retract() if not retract_success: return False @@ -496,15 +622,15 @@ def stop_tilt_callback(self, request, response): response.success = True return response - def set_push_position_callback(self, request, response): - """ - This service request sets position of the motor that pushes the auger into the ground. - It will fail if the tilt actuator is not fully extended - """ - response.success = self.set_motor_push_position( - request.speed, request.position, request.power_limit - ) - return response + # def set_push_position_callback(self, request, response): + # """ + # This service request sets position of the motor that pushes the auger into the ground. + # It will fail if the tilt actuator is not fully extended + # """ + # response.success = self.set_motor_push_position( + # request.speed, request.position, request.power_limit + # ) + # return response def stop_push_callback(self, request, response): """This service request stops the motor that pushes the auger into the ground.""" @@ -523,15 +649,18 @@ def stop_spin_callback(self, request, response): response.success = self.stop_auger_spin() return response - def push_motor_position_callback(self, msg): - self.tilt_actuator_position = msg.data + def limit_switch_callback(self, msg): + self.extension_limit_switch = msg.data + + def dumper_stowed_callback(self, msg): + self.dumper_stowed = msg.data def extend_push_callback(self, request, response): """ This service requests to extend the push motor at full speed. It will fail if the tilt actuator is not fully extended """ - response.success = self.extend_motor_push() + response.success = self.set_motor_push_extend() return response def retract_push_callback(self, request, response): @@ -539,7 +668,7 @@ def retract_push_callback(self, request, response): This service requests to retract the push motor at full speed. It will fail if the tilt actuator is not fully extended """ - response.success = self.retract_motor_push() + response.success = self.set_motor_push_retract() return response def extend_digger_callback(self, request, response): @@ -563,20 +692,15 @@ def main(args=None): rclpy.init(args=args) node = Auger() - executor = MultiThreadedExecutor() - executor.add_node(node) node.get_logger().info("Initializing the Auger subsystem!") - try: - executor.spin() - # TODO this exception needed? Could this ever happen? - except KeyboardInterrupt: - node.get_logger().info("Shutting down the Auger subsystem.") - finally: - node.destroy_node() + rclpy.spin(node) - rclpy.shutdown() + node.get_logger().info(" subsystem!") + + node.destroy_node() + rclpy.shutdown() # This code does NOT run if this file is imported as a module if __name__ == "__main__": diff --git a/src/drivetrain/drivetrain/drivetrain_node.py b/src/drivetrain/drivetrain/drivetrain_node.py index 5910eb0c..812fb4fb 100644 --- a/src/drivetrain/drivetrain/drivetrain_node.py +++ b/src/drivetrain/drivetrain/drivetrain_node.py @@ -30,7 +30,9 @@ def __init__(self): self.declare_parameter("GAZEBO_SIMULATION", False) self.declare_parameter("MAX_DRIVETRAIN_RPM", 10000) self.declare_parameter("DRIVETRAIN_TYPE", "velocity") - self.declare_parameter("DIGGER_SAFETY_ZONE", 120) # Measured in potentiometer units (0 to 1023) + self.declare_parameter( + "EXTENSION_SAFETY_ZONE", 0 + ) # Measured in potentiometer units (0 to 1023) # Assign the ROS Parameters to member variables below # self.FRONT_LEFT_DRIVE = self.get_parameter("FRONT_LEFT_DRIVE").value @@ -40,43 +42,76 @@ def __init__(self): self.GAZEBO_SIMULATION = self.get_parameter("GAZEBO_SIMULATION").value self.MAX_DRIVETRAIN_RPM = self.get_parameter("MAX_DRIVETRAIN_RPM").value self.DRIVETRAIN_TYPE = self.get_parameter("DRIVETRAIN_TYPE").value - self.DIGGER_SAFETY_ZONE = self.get_parameter("DIGGER_SAFETY_ZONE").value + self.EXTENSION_SAFETY_ZONE = self.get_parameter("EXTENSION_SAFETY_ZONE").value # Define publishers and subscribers here - self.cmd_vel_sub = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_callback, 10) - self.lift_pose_subscription = self.create_subscription(Float32, "lift_pose", self.lift_pose_callback, 10) + self.cmd_vel_sub = self.create_subscription( + Twist, "cmd_vel", self.cmd_vel_callback, 10 + ) + self.extension_sub = self.create_subscription( + Float32, "extension_pos", self.extension_callback, 10 + ) if self.GAZEBO_SIMULATION: - self.gazebo_front_left_wheel_pub = self.create_publisher(Float64, "front_left_wheel/cmd_vel", 10) - self.gazebo_back_left_wheel_pub = self.create_publisher(Float64, "back_left_wheel/cmd_vel", 10) - self.gazebo_front_right_wheel_pub = self.create_publisher(Float64, "front_right_wheel/cmd_vel", 10) - self.gazebo_back_right_wheel_pub = self.create_publisher(Float64, "back_right_wheel/cmd_vel", 10) + self.gazebo_front_left_wheel_pub = self.create_publisher( + Float64, "front_left_wheel/cmd_vel", 10 + ) + self.gazebo_back_left_wheel_pub = self.create_publisher( + Float64, "back_left_wheel/cmd_vel", 10 + ) + self.gazebo_front_right_wheel_pub = self.create_publisher( + Float64, "front_right_wheel/cmd_vel", 10 + ) + self.gazebo_back_right_wheel_pub = self.create_publisher( + Float64, "back_right_wheel/cmd_vel", 10 + ) # Define service clients here self.cli_motor_set = self.create_client(MotorCommandSet, "motor/set") # Define services (methods callable from the outside) here - self.srv_stop = self.create_service(Trigger, "drivetrain/stop", self.stop_callback) - self.srv_drive = self.create_service(Drive, "drivetrain/drive", self.drive_callback) + self.srv_stop = self.create_service( + Trigger, "drivetrain/stop", self.stop_callback + ) + self.srv_drive = self.create_service( + Drive, "drivetrain/drive", self.drive_callback + ) # Print the ROS Parameters to the terminal below # - self.get_logger().info("FRONT_LEFT_DRIVE has been set to: " + str(self.FRONT_LEFT_DRIVE)) - self.get_logger().info("FRONT_RIGHT_DRIVE has been set to: " + str(self.FRONT_RIGHT_DRIVE)) - self.get_logger().info("BACK_LEFT_DRIVE has been set to: " + str(self.BACK_LEFT_DRIVE)) - self.get_logger().info("BACK_RIGHT_DRIVE has been set to: " + str(self.BACK_RIGHT_DRIVE)) - self.get_logger().info("GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION)) - self.get_logger().info("MAX_DRIVETRAIN_RPM has been set to: " + str(self.MAX_DRIVETRAIN_RPM)) - self.get_logger().info("DIGGER_SAFETY_ZONE has been set to: " + str(self.DIGGER_SAFETY_ZONE)) + self.get_logger().info( + "FRONT_LEFT_DRIVE has been set to: " + str(self.FRONT_LEFT_DRIVE) + ) + self.get_logger().info( + "FRONT_RIGHT_DRIVE has been set to: " + str(self.FRONT_RIGHT_DRIVE) + ) + self.get_logger().info( + "BACK_LEFT_DRIVE has been set to: " + str(self.BACK_LEFT_DRIVE) + ) + self.get_logger().info( + "BACK_RIGHT_DRIVE has been set to: " + str(self.BACK_RIGHT_DRIVE) + ) + self.get_logger().info( + "GAZEBO_SIMULATION has been set to: " + str(self.GAZEBO_SIMULATION) + ) + self.get_logger().info( + "MAX_DRIVETRAIN_RPM has been set to: " + str(self.MAX_DRIVETRAIN_RPM) + ) + self.get_logger().info( + "EXTENSION_SAFETY_ZONE has been set to: " + str(self.EXTENSION_SAFETY_ZONE) + ) # Current position of the lift motor in potentiometer units (0 to 1023) - self.current_lift_position = None # We don't know the current position yet + self.extension_pos = 0 # We don't know the current position yet # Define subsystem methods here def drive(self, forward_power: float, turning_power: float) -> None: """This method drives the robot with the desired forward and turning power.""" if ( - (self.current_lift_position is None or self.current_lift_position > self.DIGGER_SAFETY_ZONE) + ( + self.extension_pos is None + or self.extension_pos > self.EXTENSION_SAFETY_ZONE + ) and not self.GAZEBO_SIMULATION and (forward_power != 0 or turning_power != 0) ): @@ -104,25 +139,45 @@ def drive(self, forward_power: float, turning_power: float) -> None: # Send velocity (not duty cycle) motor commands to the motor_control_node self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower) + MotorCommandSet.Request( + can_id=self.FRONT_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower) + MotorCommandSet.Request( + can_id=self.BACK_LEFT_DRIVE, type=self.DRIVETRAIN_TYPE, value=leftPower + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type=self.DRIVETRAIN_TYPE, value=rightPower) + MotorCommandSet.Request( + can_id=self.FRONT_RIGHT_DRIVE, + type=self.DRIVETRAIN_TYPE, + value=rightPower, + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type=self.DRIVETRAIN_TYPE, value=rightPower) + MotorCommandSet.Request( + can_id=self.BACK_RIGHT_DRIVE, + type=self.DRIVETRAIN_TYPE, + value=rightPower, + ) ) # Publish the wheel speeds to the gazebo simulation if self.GAZEBO_SIMULATION: if self.DRIVETRAIN_TYPE == "velocity": - self.gazebo_front_left_wheel_pub.publish(Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM)) - self.gazebo_back_left_wheel_pub.publish(Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM)) - self.gazebo_front_right_wheel_pub.publish(Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM)) - self.gazebo_back_right_wheel_pub.publish(Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM)) + self.gazebo_front_left_wheel_pub.publish( + Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM) + ) + self.gazebo_back_left_wheel_pub.publish( + Float64(data=leftPower / self.MAX_DRIVETRAIN_RPM) + ) + self.gazebo_front_right_wheel_pub.publish( + Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM) + ) + self.gazebo_back_right_wheel_pub.publish( + Float64(data=rightPower / self.MAX_DRIVETRAIN_RPM) + ) else: self.gazebo_front_left_wheel_pub.publish(Float64(data=leftPower)) self.gazebo_back_left_wheel_pub.publish(Float64(data=leftPower)) @@ -133,16 +188,24 @@ def drive(self, forward_power: float, turning_power: float) -> None: def stop(self) -> None: """This method stops the drivetrain.""" self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=0.0) + MotorCommandSet.Request( + can_id=self.FRONT_LEFT_DRIVE, type="duty_cycle", value=0.0 + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=0.0) + MotorCommandSet.Request( + can_id=self.BACK_LEFT_DRIVE, type="duty_cycle", value=0.0 + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=0.0) + MotorCommandSet.Request( + can_id=self.FRONT_RIGHT_DRIVE, type="duty_cycle", value=0.0 + ) ) self.cli_motor_set.call_async( - MotorCommandSet.Request(can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=0.0) + MotorCommandSet.Request( + can_id=self.BACK_RIGHT_DRIVE, type="duty_cycle", value=0.0 + ) ) # Define service callback methods here @@ -165,9 +228,9 @@ def cmd_vel_callback(self, msg: Twist) -> None: self.drive(msg.linear.x, msg.angular.z) # Define the subscriber callback for the lift pose topic - def lift_pose_callback(self, msg: Float32): + def extension_callback(self, msg: Float32): # Average the two potentiometer values - self.current_lift_position = msg.data + self.extension_pos = msg.data def main(args=None): diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index 48aa0470..f0d17298 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -34,10 +34,16 @@ def __init__(self): # Define services (methods callable from the outside) here self.srv_toggle = self.create_service( - Trigger, "dumper/toggle", self.toggle_callback, callback_group=self.service_cb_group + Trigger, + "dumper/toggle", + self.toggle_callback, + callback_group=self.service_cb_group, ) self.srv_stop = self.create_service( - Trigger, "dumper/stop", self.stop_callback, callback_group=self.stop_service_cb_group + Trigger, + "dumper/stop", + self.stop_callback, + callback_group=self.stop_service_cb_group, ) self.srv_setPower = self.create_service( SetPower, @@ -47,21 +53,25 @@ def __init__(self): ) self.srv_dumpDumper = self.create_service( - Trigger, "dumper/storeDumper", self.dump_callback, callback_group=self.service_cb_group + Trigger, "dumper/dumpDumper", self.dump_callback, callback_group=self.service_cb_group ) self.srv_storeDumper = self.create_service( - Trigger, "dumper/dumpDumper", self.store_callback, callback_group=self.service_cb_group + Trigger, "dumper/storeDumper", self.store_callback, callback_group=self.service_cb_group ) # Define default values for our ROS parameters below # - self.declare_parameter("DUMPER_MOTOR", 11) - self.declare_parameter("DUMPER_POWER", 0.3) + self.declare_parameter("DUMPER_MOTOR", 2) + self.declare_parameter("DUMPER_POWER", 0.5) + self.declare_parameter("DUMPER_VELOCITY", 0.0) # Assign the ROS Parameters to member variables below # self.DUMPER_MOTOR = self.get_parameter("DUMPER_MOTOR").value self.DUMPER_POWER = self.get_parameter("DUMPER_POWER").value + self.DUMPER_VEL = self.get_parameter("DUMPER_VELOCITY").value # Print the ROS Parameters to the terminal below # - self.get_logger().info("DUMPER_MOTOR has been set to: " + str(self.DUMPER_MOTOR)) + self.get_logger().info( + "DUMPER_MOTOR has been set to: " + str(self.DUMPER_MOTOR) + ) # Current state of the dumper self.dumped_state = False @@ -77,8 +87,16 @@ def __init__(self): self.KillSwitch_sub = self.create_subscription( Bool, "DumperLimitSwitch", self.killSwitch_callback, 10 ) + + self.auger_stowed_sub = self.create_subscription( + Bool, "auger_stowed", self.auger_stowed_callback, 10 + ) + + self.dumper_stowed_pub = self.create_publisher(Bool, "dumper_stowed", 10) + + self.dumper_stowed = True self.limitSwitchBottom = False - # self.auger_stowed = True + self.auger_stowed = True # Define subsystem methods here def set_power(self, dumper_power: float) -> None: @@ -89,11 +107,17 @@ def set_power(self, dumper_power: float) -> None: ) ) - def stop(self) -> None: + def stop(self) -> bool: """This method stops the dumper.""" - self.cli_motor_set.call_async( - MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=0.0) + stop_dumper = self.cli_motor_set.call_async( + MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=float(0.0)) ) + self.get_logger().info("Waiting to stop the dumper...") + rclpy.spin_until_future_complete(self, stop_dumper) + self.get_logger().info("Done waiting to stop the dumper!") + if not stop_dumper.result().success: + self.get_logger().error("Failed to stop the dumper") + return stop_dumper.result().success def toggle(self) -> None: """This method toggles the dumper.""" @@ -124,59 +148,97 @@ def toggle_callback(self, request, response): return response def dump_dumper(self) -> None: - # if not self.auger_stowed: - # self.get_logger().info("The auger is already extended") - # return + if not self.auger_stowed: + self.get_logger().info("The auger is already extended") + return self.get_logger().info("Extending the dumper") self.dumped_state = True self.long_service_running = True future = self.cli_motor_set.call_async( - MotorCommandSet.Request(type="position", can_id=self.DUMPER_MOTOR, value=90) + MotorCommandSet.Request(type="velocity", can_id=self.DUMPER_MOTOR, value=self.DUMPER_VEL) ) - while not future.done(): # While loop makes the motor keep going till limit switch is hit + self.dumper_stowed = False + msg = Bool() + msg.data = self.dumper_stowed + self.dumper_stowed_pub.publish(msg) + while ( + not future.done() + ): # While loop makes the motor keep going till limit switch is hit if self.cancel_current_srv: self.cancel_current_srv = False break time.sleep(0.1) + + while True: + motor_get_future = self.cli_motor_get.call_async( + MotorCommandGet.Request( + type="position", + can_id=self.DUMPER_MOTOR, + ) + ) + rclpy.spin_until_future_complete(self, motor_get_future) + if motor_get_future.result().success: + if ( + abs(motor_get_future.result().data - self.TILT_ACTUATOR_CURRENT_THRESHOLD.value) < 5 + ): + break + else: + self.get_logger().info("WARNING: Failed to read tilt actuator position") + + time.sleep(0.1) self.stop() self.long_service_running = False self.get_logger().info("Done dumping the dumper") def dump_callback(self, request, response): - self.dump_dumper() - response.success = True - return response + return self.dump_dumper() - def store_dumper(self) -> None: # get the variables - # if not self.auger_stowed: - # self.get_logger().info("The Auger is already extended") - # return + def store_dumper(self) -> bool: # get the variables + if not self.auger_stowed: + self.get_logger().info("The Auger is already extended") + return self.get_logger().info("Retracting the dumper") self.dumped_state = False self.long_service_running = True - self.cli_motor_set.call_async( + store_dumper = self.cli_motor_set.call_async( MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=self.DUMPER_POWER) ) + rclpy.spin_until_future_complete(self, store_dumper) + if not store_dumper.result().success: + self.get_logger().error("Failed to start storing the dumper") + self.long_service_running = False + return False while not self.limitSwitchBottom: if self.cancel_current_srv: self.cancel_current_srv = False break time.sleep(0.1) + time.sleep(2.0) + + stop_success = self.stop() + if not stop_success: + return False + self.stop() + self.dumper_stowed = True + msg = Bool() + msg.data = self.dumper_stowed + self.dumper_stowed_pub.publish(msg) self.long_service_running = False self.get_logger().info("Done storing the dumper") + return True + + # the storage bin can only be dumped back # when the auger is completely stowed (both actuators fully stored) def store_callback(self, request, response): - self.store_dumper() - response.success = True - return response + return self.store_dumper() def dumper_current_callback(self, msg): self.dumper_current = msg.data @@ -185,6 +247,9 @@ def killSwitch_callback(self, msg): # position control... self.LimitSwitchBottom = msg.data + def auger_stowed_callback(self, msg): + self.auger_stowed = msg.data + def main(args=None): """The main function.""" diff --git a/src/rovr_control/rovr_control/auto_dig_server.py b/src/rovr_control/rovr_control/auto_dig_server.py index cd41d8ee..221941c3 100644 --- a/src/rovr_control/rovr_control/auto_dig_server.py +++ b/src/rovr_control/rovr_control/auto_dig_server.py @@ -35,9 +35,9 @@ def __init__(self): ) # /actuator_tilt/stop # extend - self.set_extension = self.create_client( - AugerSetPushMotor, "auger/push_motor/setPosition" - ) + # self.set_extension = self.create_client( + # AugerSetPushMotor, "auger/push_motor/setPosition" + # ) self.stop_extension = self.create_client(Trigger, "auger/push_motor/stop") self.retract_extender = self.create_client(Trigger, "auger/push_motor/retract") @@ -85,10 +85,10 @@ async def execute_callback(self, goal_handle: ServerGoalHandle): self.get_logger().error("Tilt stop service not available") goal_handle.abort() return result - if not self.set_extension.wait_for_service(timeout_sec=1.0): - self.get_logger().error("Extension set power service not available") - goal_handle.abort() - return result + # if not self.set_extension.wait_for_service(timeout_sec=1.0): + # self.get_logger().error("Extension set power service not available") + # goal_handle.abort() + # return result if not self.retract_extender.wait_for_service(timeout_sec=1.0): self.get_logger().error("Extension retract service not available") goal_handle.abort() diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 4d3b988a..6331460b 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -204,9 +204,6 @@ def __init__(self) -> None: 10, callback_group=ReentrantCallbackGroup(), ) - self.lift_pose_subscription = self.create_subscription( - Float32, "lift_pose", self.lift_pose_callback, 10 - ) self.act_calibrate_field_coordinates = ActionClient( self, CalibrateFieldCoordinates, "calibrate_field_coordinates" @@ -226,9 +223,6 @@ def __init__(self) -> None: None, None, None ) - # Current position of the lift motor in potentiometer units (0 to 1023) - self.current_lift_position = None # We don't know the current position yet - # Add watchdog parameters self.declare_parameter("watchdog_timeout", 0.5) # Timeout in seconds self.watchdog_timeout = self.get_parameter("watchdog_timeout").value @@ -557,10 +551,6 @@ async def stream_deck_callback(self, msg: StreamDeckState) -> None: # self.get_logger().warn("Joystick messages received! Functionality of the robot has been restored.") # self.connection_active = True - # Define the subscriber callback for the lift pose topic - def lift_pose_callback(self, msg: Float32): - # Average the two potentiometer values - self.current_lift_position = msg.data def main(args=None) -> None: diff --git a/src/rovr_control/rovr_control/read_serial.py b/src/rovr_control/rovr_control/read_serial.py index 1d5e83fe..517e0a88 100644 --- a/src/rovr_control/rovr_control/read_serial.py +++ b/src/rovr_control/rovr_control/read_serial.py @@ -12,8 +12,10 @@ class read_serial(Node): def __init__(self): super().__init__("read_serial") - self.potentiometerPub = self.create_publisher(Int16, "potentiometer", 10) - self.LimitSwitchPub = self.create_publisher(Bool, "DumperLimitSwitch", 10) + self.DumperLimitSwitchPub = self.create_publisher(Bool, "DumperLimitSwitch", 10) + self.ExtensionLimitSwitchPub = self.create_publisher( + Bool, "ExtensionLimitSwitch", 10 + ) # Services to control the relay-driven agitator motor self.srv_bigonoff = self.create_service( @@ -50,17 +52,19 @@ def timer_callback(self): self.get_logger().fatal("Killing read_serial node") self.destroy_node() return - data = self.arduino.read(4) # Pause until 4 bytes are read + data = self.arduino.read(2) # Pause until 4 bytes are read # Use h for integers and ? for booleans - decoded = struct.unpack("h?", data) + decoded = struct.unpack("??", data) + + dumper_bool_msg = Bool() + dumper_bool_msg.data = not decoded[0] + self.DumperLimitSwitchPub.publish(dumper_bool_msg) - potentiometer_msg = Int16() - potentiometer_msg.data = decoded[0] - self.potentiometerPub.publish(potentiometer_msg) + extension_bool_msg = Bool() + extension_bool_msg.data = not decoded[1] + self.ExtensionLimitSwitchPub.publish(extension_bool_msg) - bool_msg = Bool() - bool_msg.data = decoded[1] - self.LimitSwitchPub.publish(bool_msg) + # self.get_logger().info(f"dumper bool: {not decoded[0]}, extension bool: {not decoded[1]}") def big_on_off_callback(self, request, response): # request.data == True → ON, False → OFF diff --git a/src/rovr_control/rovr_control/xbox_controller_constants.py b/src/rovr_control/rovr_control/xbox_controller_constants.py index 30ed9517..1e05f649 100644 --- a/src/rovr_control/rovr_control/xbox_controller_constants.py +++ b/src/rovr_control/rovr_control/xbox_controller_constants.py @@ -18,5 +18,5 @@ RIGHT_JOYSTICK_VERTICAL_AXIS = 4 # drive LEFT_TRIGGER_AXIS = 2 # extend auger RIGHT_TRIGGER_AXIS = 5 # retract auger -DPAD_HORIZONTAL_AXIS = 6 # FREE +DPAD_HORIZONTAL_AXIS = 6 # DUMP DPAD_VERTICAL_AXIS = 7 # FREE From 03296d91aa68ec9e3ac1f3f5790c20b46434afaa Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Mon, 27 Apr 2026 14:39:08 -0500 Subject: [PATCH 18/21] afternoon tests --- config/auger_config.yaml | 2 +- src/auger/auger/auger_node.py | 98 +++++++++++++------ src/dumper/dumper/dumper_node.py | 14 +-- src/motor_control/src/motor_control_node.cpp | 4 + .../rovr_control/main_control_node.py | 12 ++- 5 files changed, 86 insertions(+), 44 deletions(-) diff --git a/config/auger_config.yaml b/config/auger_config.yaml index 99067836..6b5d5bd5 100644 --- a/config/auger_config.yaml +++ b/config/auger_config.yaml @@ -3,7 +3,7 @@ #TODO: Figure out all of these values SCREW_SPEED: 4000 # TODO: update POWER_LIMIT: 1 #TODO: update - FAST_SCREW_SPEED:LD: 0.0 #0.21 + FAST_SCREW_SPEED: 8000 #0.21 extension_limit_switch: true STOWED: true MAX_PUSH_MOTOR_POSITION: 10000 diff --git a/src/auger/auger/auger_node.py b/src/auger/auger/auger_node.py index fb49601b..c333082d 100644 --- a/src/auger/auger/auger_node.py +++ b/src/auger/auger/auger_node.py @@ -30,6 +30,7 @@ def __init__(self): # Callback group for anything that changes motor speeds # There should not be a need for a separate one because all services # should eventually terminate timely + self.stop_service_cb_group = MutuallyExclusiveCallbackGroup() self.service_cb_group = MutuallyExclusiveCallbackGroup() # TODO Define service clients here @@ -73,7 +74,7 @@ def __init__(self): self.declare_parameter("TILT_ACTUATOR_ID", 1) self.declare_parameter("PUSH_MOTOR_ID", 0) self.declare_parameter("SPIN_MOTOR_ID", 0) - self.declare_parameter("FAST_SCREW_SPEED", 3000) + self.declare_parameter("FAST_SCREW_SPEED", 8000) self.declare_parameter("SLOW_SCREW_SPEED", 0) # Local variables here @@ -123,6 +124,11 @@ def __init__(self): self.cancel_linear_actuator: bool = False self.linear_actuator_running: bool = False + self.cancel_motor_push: bool = False + self.motor_push_running: bool = False + + + # TODO Define services (methods callable from the outside) here self.srv_set_tilt_extension = self.create_service( SetExtension, @@ -135,7 +141,7 @@ def __init__(self): Trigger, "auger/tilt_actuator/stop", self.stop_tilt_callback, - callback_group=self.service_cb_group, + callback_group=self.stop_service_cb_group, ) # self.srv_set_push_position = self.create_service( @@ -149,7 +155,7 @@ def __init__(self): Trigger, "auger/push_motor/stop", self.stop_push_callback, - callback_group=self.service_cb_group, + callback_group=self.stop_service_cb_group, ) self.srv_extend_push = self.create_service( @@ -177,7 +183,7 @@ def __init__(self): Trigger, "auger/screw/stop", self.stop_spin_callback, - callback_group=self.service_cb_group, + callback_group=self.stop_service_cb_group, ) self.srv_extend_digger = self.create_service( @@ -198,7 +204,7 @@ def __init__(self): Trigger, "auger/control/stop_all", self.stop_all_callback, - callback_group=self.service_cb_group, + callback_group=self.stop_service_cb_group, ) @@ -213,7 +219,7 @@ def __init__(self): # TODO Define publishers here self.auger_stowed_pub = self.create_publisher(Bool, "auger_stowed", 10) - self.extension_pos_sub = self.create_publisher(Float32, "extension_pos", 10) + self.extension_pos_pub = self.create_publisher(Float32, "extension_pos", 10) # Define subsystem methods here @@ -280,7 +286,7 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: ) rclpy.spin_until_future_complete(self, motor_get_future) if motor_get_future.result().success: - self.get_logger().info(f"Actuator current: {motor_get_future.result().data}") + # self.get_logger().info(f"Actuator current: {motor_get_future.result().data}") if ( abs(motor_get_future.result().data) < self.TILT_ACTUATOR_CURRENT_THRESHOLD @@ -292,6 +298,7 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: time.sleep(0.1) if self.cancel_linear_actuator: + self.get_logger().info("Cancelling the linear actuator") break @@ -317,6 +324,7 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: def stop_actuator_tilt(self) -> bool: """Stop the auger angular position of the auger motor.""" + self.get_logger().info("In the stop acutator tilt function") if self.linear_actuator_running: self.cancel_linear_actuator = True return True @@ -339,31 +347,36 @@ def set_motor_push_extend(self) -> bool: This will fail if the screw is not spinning fast enough Caller is responsible for timeouts. """ + self.motor_push_running = True power_limit = 0.5 if not self.linear_actuator_tilted: self.get_logger().warn( "WARNING: Push motor will not move because the tilt actuator is not extended" ) + self.motor_push_running = False return False - get_screw_speed_future = self.cli_motor_get.call_async( - MotorCommandGet.Request( - type="velocity", - can_id=self.SPIN_MOTOR_ID, - ) - ) - rclpy.spin_until_future_complete(self, get_screw_speed_future) + # get_screw_speed_future = self.cli_motor_get.call_async( + # MotorCommandGet.Request( + # type="velocity", + # can_id=self.SPIN_MOTOR_ID, + # ) + # ) + # rclpy.spin_until_future_complete(self, get_screw_speed_future) - if not get_screw_speed_future.result().success: - self.get_logger().warn( - "WARNING: Push motor will not move because the screw speed could not be determined" - ) - return False - elif get_screw_speed_future.result().data < self.MIN_SCREW_DIG_SPEED: - self.get_logger().warn( - "WARNING: Push motor will not move because the screw is not spinning fast enough" - ) - return False + # if not get_screw_speed_future.result().success: + # self.get_logger().warn( + # "WARNING: Push motor will not move because the screw speed could not be determined" + # ) + # self.motor_push_running = False + # return False + + # elif get_screw_speed_future.result().data < self.MIN_SCREW_DIG_SPEED: + # self.get_logger().warn( + # "WARNING: Push motor will not move because the screw is not spinning fast enough" + # ) + # self.motor_push_running = False + # return False if ( self.MAX_PUSH_MOTOR_POSITION > self.MAX_PUSH_MOTOR_POSITION @@ -379,23 +392,32 @@ def set_motor_push_extend(self) -> bool: self.get_logger().info( "Setting auger push motor position to: " + str(self.MAX_PUSH_MOTOR_POSITION) ) + motor_set_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="velocity", - power_limit=power_limit, can_id=self.PUSH_MOTOR_ID, value=float(self.DEFAULT_PUSH_MOTOR_SPEED), ) ) + self.get_logger().info( + f"Set the plunge velocity to {self.DEFAULT_PUSH_MOTOR_SPEED}" + ) rclpy.spin_until_future_complete(self, motor_set_future) if not motor_set_future.result().success: self.get_logger().warn("WARNING: Failed to set push motor voltage") + self.motor_push_running = False return False # wait till motor reaches desired position while True: + if self.cancel_motor_push: + break + self.get_logger().info( + f"getting the plunge velocity" + ) motor_get_pos_future = self.cli_motor_get.call_async( MotorCommandGet.Request( type="position", @@ -408,9 +430,9 @@ def set_motor_push_extend(self) -> bool: current_pos = motor_get_pos_future.result().data msg = Float32() msg.data = current_pos - self.extension_pos_sub.publish(msg) + self.extension_pos_pub.publish(msg) if ( - (self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MAX_PUSH_MOTOR_POSITION) + (self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MIN_PUSH_MOTOR_POSITION) or (self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MAX_PUSH_MOTOR_POSITION) ): break @@ -427,11 +449,14 @@ def set_motor_push_extend(self) -> bool: value=0, ) ) + self.motor_push_running = False + self.cancel_motor_push = False stop_motor_response = rclpy.spin_until_future_complete(self, stop_motor_future) if not stop_motor_response.result().success: self.get_logger().warn("WARNING: Failed to stop the auger screw") return False + return True def set_motor_push_retract(self) -> bool: @@ -441,6 +466,7 @@ def set_motor_push_retract(self) -> bool: This will fail if the screw is not spinning fast enough Caller is responsible for timeouts. """ + self.motor_push_running = True power_limit = 0.5 if ( @@ -470,10 +496,14 @@ def set_motor_push_retract(self) -> bool: if not motor_set_future.result().success: self.get_logger().warn("WARNING: Failed to set push motor voltage") + self.motor_push_running = False return False # wait till motor reaches desired position while True: + if self.cancel_motor_push: + break + motor_get_pos_future = self.cli_motor_get.call_async( MotorCommandGet.Request( type="position", @@ -486,10 +516,10 @@ def set_motor_push_retract(self) -> bool: current_pos = motor_get_pos_future.result().data msg = Float32() msg.data = current_pos - self.extension_pos_sub.publish(msg) + self.extension_pos_pub.publish(msg) if ( (-self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MIN_PUSH_MOTOR_POSITION) - or (-self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MIN_PUSH_MOTOR_POSITION) + or (-self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MAX_PUSH_MOTOR_POSITION) or (self.extension_limit_switch) ): break @@ -506,6 +536,8 @@ def set_motor_push_retract(self) -> bool: value=0, ) ) + self.motor_push_running = False + self.cancel_motor_push = False stop_motor_response = rclpy.spin_until_future_complete(self, stop_motor_future) if not stop_motor_response.result().success: self.get_logger().warn("WARNING: Failed to stop the auger screw") @@ -515,6 +547,8 @@ def set_motor_push_retract(self) -> bool: def stop_motor_push(self) -> bool: """Stop the motor that pushes the auger into the ground.""" + if self.motor_push_running: + self.cancel_motor_push = True motor_set_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", @@ -573,7 +607,9 @@ def extend_digger(self) -> bool: return False tilt_success = self.set_actuator_tilt_extension(True) - if not tilt_success: + + if not tilt_success or self.cancel_linear_actuator: + self.cancel_linear_actuator = False return False spin_success = self.run_auger_spin_velocity(self.FAST_SCREW_SPEED, self.POWER_LIMIT) @@ -606,6 +642,7 @@ def retract_digger(self) -> bool: def stop_all(self) -> bool: # This does not short circit but still returns false if any do + self.get_logger().info("in the stop all function") return ( self.stop_actuator_tilt() & self.stop_auger_spin() & self.stop_motor_push() ) @@ -683,6 +720,7 @@ def retract_digger_callback(self, request, response): def stop_all_callback(self, request, response): """This Service will stop all three motors""" + self.get_logger().info("In the stop all callback") response.success = self.stop_all() return response diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index f0d17298..aeeafcd7 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -62,7 +62,7 @@ def __init__(self): # Define default values for our ROS parameters below # self.declare_parameter("DUMPER_MOTOR", 2) self.declare_parameter("DUMPER_POWER", 0.5) - self.declare_parameter("DUMPER_VELOCITY", 0.0) + self.declare_parameter("DUMPER_VELOCITY", 1200*7) # Assign the ROS Parameters to member variables below # self.DUMPER_MOTOR = self.get_parameter("DUMPER_MOTOR").value self.DUMPER_POWER = self.get_parameter("DUMPER_POWER").value @@ -157,23 +157,19 @@ def dump_dumper(self) -> None: self.long_service_running = True future = self.cli_motor_set.call_async( - MotorCommandSet.Request(type="velocity", can_id=self.DUMPER_MOTOR, value=self.DUMPER_VEL) + MotorCommandSet.Request(type="velocity", can_id=-self.DUMPER_MOTOR, value=self.DUMPER_VEL) ) self.dumper_stowed = False msg = Bool() msg.data = self.dumper_stowed self.dumper_stowed_pub.publish(msg) - while ( - not future.done() - ): # While loop makes the motor keep going till limit switch is hit - if self.cancel_current_srv: - self.cancel_current_srv = False - break - time.sleep(0.1) while True: + if self.cancel_current_srv: + self.cancel_current_srv = False + break motor_get_future = self.cli_motor_get.call_async( MotorCommandGet.Request( type="position", diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index 13321d18..2e679b19 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -411,12 +411,16 @@ class MotorControlNode : public rclcpp::Node { std::shared_ptr response) { std::optional data = std::nullopt; + // RCLCPP_INFO(this->get_logger(), "GET COMMAND IS CALLED"); + if (strcmp(request->type.c_str(), "velocity") == 0) { data = vesc_get_velocity(request->can_id); } else if (strcmp(request->type.c_str(), "duty_cycle") == 0) { data = vesc_get_duty_cycle(request->can_id); } else if (strcmp(request->type.c_str(), "position") == 0) { + // RCLCPP_INFO(this->get_logger(), "USING POSITION CALLBACK"); data = vesc_get_position(request->can_id); + // RCLCPP_INFO(this->get_logger(), "looked up data succcessfuly"); } else if (strcmp(request->type.c_str(), "current") == 0) { data = vesc_get_current(request->can_id); } else { diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 6331460b..1051d098 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -159,6 +159,9 @@ def __init__(self) -> None: self.cli_dumper_toggle = self.create_client(Trigger, "dumper/toggle") self.cli_dumper_setPower = self.create_client(SetPower, "dumper/setPower") self.cli_dumper_stop = self.create_client(Trigger, "dumper/stop") + + self.cli_dumper_dumpDumper = self.create_client(Trigger, "dump/dumpDumper") + self.cli_dumper_storeDumper = self.create_client(Trigger, "dump/storeDumper") # self.cli_digger_toggle = self.create_client(SetPower, "digger/toggle") self.cli_auger_stop = self.create_client(Trigger, "auger/control/stop_all") self.cli_auger_extend = self.create_client( @@ -411,8 +414,8 @@ async def joystick_callback(self, msg: Joy) -> None: msg.buttons[bindings.RIGHT_BUMPER] == 1 and buttons[bindings.RIGHT_BUMPER] == 0 ): - self.cli_dumper_setPower.call_async( - SetPower.Request(power=self.dumper_power) + self.cli_dumper_dumpDumper.call_async( + Trigger.Request() ) elif ( msg.buttons[bindings.RIGHT_BUMPER] == 0 @@ -423,8 +426,8 @@ async def joystick_callback(self, msg: Joy) -> None: msg.buttons[bindings.LEFT_BUMPER] == 1 and buttons[bindings.LEFT_BUMPER] == 0 ): - self.cli_dumper_setPower.call_async( - SetPower.Request(power=-self.dumper_power) + self.cli_dumper_storeDumper.call_async( + Trigger.Request() ) elif ( msg.buttons[bindings.LEFT_BUMPER] == 0 @@ -443,6 +446,7 @@ async def joystick_callback(self, msg: Joy) -> None: msg.buttons[bindings.LEFT_TRIGGER] == 0 and buttons[bindings.LEFT_TRIGGER] == 1 ): + self.get_logger().info("In main control node stop") self.cli_auger_stop.call_async(Trigger.Request()) elif ( msg.buttons[bindings.RIGHT_TRIGGER] == 1 From 36169cb7eeeac28dfa706cd07f269a077783df16 Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Mon, 27 Apr 2026 22:41:14 -0500 Subject: [PATCH 19/21] testing the tilt, extend, and dump --- config/auger_config.yaml | 4 +- config/rovr_control.yaml | 2 +- src/auger/auger/auger_node.py | 77 ++++++++++--------- src/dumper/dumper/dumper_node.py | 21 +++-- src/motor_control/src/motor_control_node.cpp | 30 ++++++-- .../rovr_control/main_control_node.py | 67 ++++++++-------- .../rovr_control/xbox_controller_constants.py | 8 +- 7 files changed, 125 insertions(+), 84 deletions(-) diff --git a/config/auger_config.yaml b/config/auger_config.yaml index 6b5d5bd5..d88c84ea 100644 --- a/config/auger_config.yaml +++ b/config/auger_config.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: #TODO: Figure out all of these values - SCREW_SPEED: 4000 # TODO: update + SCREW_SPEED: 20000 # TODO: update POWER_LIMIT: 1 #TODO: update - FAST_SCREW_SPEED: 8000 #0.21 + FAST_SCREW_SPEED: 20000 #0.21 extension_limit_switch: true STOWED: true MAX_PUSH_MOTOR_POSITION: 10000 diff --git a/config/rovr_control.yaml b/config/rovr_control.yaml index cb6e909e..4cefe568 100644 --- a/config/rovr_control.yaml +++ b/config/rovr_control.yaml @@ -7,7 +7,7 @@ digger_lift_manual_power_up: 0.5 # Measured in Duty Cycle (0.0-1.0) autonomous_field_type: "cosmic" # The type of field ("cosmic", "top", "bottom", "nasa") tilt_digging_start_position: 125.0 # Measured in potentiometer units (0 to 1023) - fast_screw_speed: 4000 + fast_screw_speed: 20000 slow_screw_speed: 2000 # Auto Dig cost starts at max_dig_cost, and increases to absolute_max_dig_cost absolute_max_dig_cost: 200 # Measured as a Costmap grid value (0-255) # TODO: tune this diff --git a/src/auger/auger/auger_node.py b/src/auger/auger/auger_node.py index c333082d..201a0e10 100644 --- a/src/auger/auger/auger_node.py +++ b/src/auger/auger/auger_node.py @@ -42,7 +42,7 @@ def __init__(self): self.declare_parameter("DUMPER_STOWED", True) self.declare_parameter("extension_limit_switch", True) self.declare_parameter("POWER_LIMIT", 1) - self.declare_parameter("SCREW_SPEED", 4000) + self.declare_parameter("SCREW_SPEED", 20000) self.declare_parameter( "MAX_SCREW_SPEED", 4_000 ) # in RPM for both negative and positive direction @@ -50,7 +50,7 @@ def __init__(self): self.declare_parameter("MAX_SPIN_MOTOR_CURRENT", 0) self.declare_parameter("push_motor_position", 0) # The error range to consider the current 0 - self.declare_parameter("TILT_ACTUATOR_CURRENT_THRESHOLD", 0.21) + self.declare_parameter("TILT_ACTUATOR_CURRENT_THRESHOLD", 0.3) # TODO: Find real value for this self.declare_parameter("MAX_PUSH_MOTOR_POSITION", 10000) self.declare_parameter("MIN_PUSH_MOTOR_POSITION", 1000) @@ -74,7 +74,7 @@ def __init__(self): self.declare_parameter("TILT_ACTUATOR_ID", 1) self.declare_parameter("PUSH_MOTOR_ID", 0) self.declare_parameter("SPIN_MOTOR_ID", 0) - self.declare_parameter("FAST_SCREW_SPEED", 8000) + self.declare_parameter("FAST_SCREW_SPEED", 20000) self.declare_parameter("SLOW_SCREW_SPEED", 0) # Local variables here @@ -286,11 +286,12 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: ) rclpy.spin_until_future_complete(self, motor_get_future) if motor_get_future.result().success: - # self.get_logger().info(f"Actuator current: {motor_get_future.result().data}") + self.get_logger().info(f"Actuator current: {motor_get_future.result().data}") if ( abs(motor_get_future.result().data) < self.TILT_ACTUATOR_CURRENT_THRESHOLD ): + self.get_logger().info(f"Exiting because current read {motor_get_future.result().data}") break else: self.get_logger().info("WARNING: Failed to read tilt actuator position") @@ -314,11 +315,13 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: return False if not tilt: self.auger_stowed = True + self.get_logger().info("Set auger_stowed") msg = Bool() msg.data = self.auger_stowed self.auger_stowed_pub.publish(msg) self.linear_actuator_tilted = tilt + self.get_logger().info(f"linear actuator tilt set to {self.linear_actuator_tilted}") return True @@ -349,12 +352,12 @@ def set_motor_push_extend(self) -> bool: """ self.motor_push_running = True power_limit = 0.5 - if not self.linear_actuator_tilted: - self.get_logger().warn( - "WARNING: Push motor will not move because the tilt actuator is not extended" - ) - self.motor_push_running = False - return False + # if not self.linear_actuator_tilted: + # self.get_logger().warn( + # "WARNING: Push motor will not move because the tilt actuator is not extended" + # ) + # self.motor_push_running = False + # return False # get_screw_speed_future = self.cli_motor_get.call_async( # MotorCommandGet.Request( @@ -393,7 +396,7 @@ def set_motor_push_extend(self) -> bool: "Setting auger push motor position to: " + str(self.MAX_PUSH_MOTOR_POSITION) ) - + self.get_logger().info("Setting the plunge motor velocity") motor_set_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="velocity", @@ -413,34 +416,34 @@ def set_motor_push_extend(self) -> bool: # wait till motor reaches desired position while True: + self.get_logger().info(f"waiting for cancel motor push, cancel_motor_push is {self.cancel_motor_push}") if self.cancel_motor_push: break - self.get_logger().info( - f"getting the plunge velocity" - ) - motor_get_pos_future = self.cli_motor_get.call_async( - MotorCommandGet.Request( - type="position", - can_id=self.PUSH_MOTOR_ID, - ) - ) - rclpy.spin_until_future_complete(self, motor_get_pos_future) - - if motor_get_pos_future.result().success: - current_pos = motor_get_pos_future.result().data - msg = Float32() - msg.data = current_pos - self.extension_pos_pub.publish(msg) - if ( - (self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MIN_PUSH_MOTOR_POSITION) - or (self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MAX_PUSH_MOTOR_POSITION) - ): - break - else: - self.get_logger().warn("WARNING: Failed to read push motor position") + # self.get_logger().info("Getting the plunge motor position") + # motor_get_pos_future = self.cli_motor_get.call_async( + # MotorCommandGet.Request( + # type="position", + # can_id=self.PUSH_MOTOR_ID, + # ) + # ) + # rclpy.spin_until_future_complete(self, motor_get_pos_future) + + # if motor_get_pos_future.result().success: + # current_pos = motor_get_pos_future.result().data + # self.get_logger().info(f"Plunge pos: {current_pose}") + # msg = Float32() + # msg.data = current_pos + # self.extension_pos_pub.publish(msg) + # if ( + # (self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MIN_PUSH_MOTOR_POSITION) + # or (self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MAX_PUSH_MOTOR_POSITION) + # ): + # break + # else: + # self.get_logger().warn("WARNING: Failed to read push motor position") time.sleep(0.1) - + self.get_logger().info("out of the loop") stop_motor_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="velocity", @@ -547,8 +550,12 @@ def set_motor_push_retract(self) -> bool: def stop_motor_push(self) -> bool: """Stop the motor that pushes the auger into the ground.""" + self.get_logger().info("In the stop acutator tilt function") if self.motor_push_running: self.cancel_motor_push = True + self.get_logger.info(f"cancel_motor_push is {self.cancel_motor_push}") + return True + motor_set_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index aeeafcd7..ba8b66cc 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -62,12 +62,13 @@ def __init__(self): # Define default values for our ROS parameters below # self.declare_parameter("DUMPER_MOTOR", 2) self.declare_parameter("DUMPER_POWER", 0.5) - self.declare_parameter("DUMPER_VELOCITY", 1200*7) + self.declare_parameter("DUMPER_VELOCITY", (1200*7)) + self.declare_parameter("DUMPER_POS", 1000) # Assign the ROS Parameters to member variables below # self.DUMPER_MOTOR = self.get_parameter("DUMPER_MOTOR").value self.DUMPER_POWER = self.get_parameter("DUMPER_POWER").value self.DUMPER_VEL = self.get_parameter("DUMPER_VELOCITY").value - + self.DUMP_POS = self.get_parameter("DUMPER_POS").value # Print the ROS Parameters to the terminal below # self.get_logger().info( "DUMPER_MOTOR has been set to: " + str(self.DUMPER_MOTOR) @@ -135,8 +136,12 @@ def set_power_callback(self, request, response): def stop_callback(self, request, response): """This service request stops the dumper.""" + self.get_logger().info("In the stop acutator tilt function") if self.long_service_running: self.cancel_current_srv = True + self.get_logger().info(f"inside if statement, cancel_current_srv: {self.cancel_current_srv}") + return True + self.stop() response.success = True return response @@ -157,7 +162,7 @@ def dump_dumper(self) -> None: self.long_service_running = True future = self.cli_motor_set.call_async( - MotorCommandSet.Request(type="velocity", can_id=-self.DUMPER_MOTOR, value=self.DUMPER_VEL) + MotorCommandSet.Request(type="velocity", can_id=self.DUMPER_MOTOR, value=float(self.DUMPER_VEL)) ) self.dumper_stowed = False @@ -167,8 +172,9 @@ def dump_dumper(self) -> None: while True: + self.get_logger().info(str(self.cancel_current_srv)) if self.cancel_current_srv: - self.cancel_current_srv = False + self.get_logger().info("cancel current srv is true") break motor_get_future = self.cli_motor_get.call_async( MotorCommandGet.Request( @@ -179,15 +185,17 @@ def dump_dumper(self) -> None: rclpy.spin_until_future_complete(self, motor_get_future) if motor_get_future.result().success: if ( - abs(motor_get_future.result().data - self.TILT_ACTUATOR_CURRENT_THRESHOLD.value) < 5 + abs(motor_get_future.result().data - self.DUMP_POS) < 5 ): break else: self.get_logger().info("WARNING: Failed to read tilt actuator position") time.sleep(0.1) + self.get_logger().info("Finished dump dumper service") self.stop() self.long_service_running = False + self.cancel_current_srv = False self.get_logger().info("Done dumping the dumper") def dump_callback(self, request, response): @@ -201,7 +209,7 @@ def store_dumper(self) -> bool: # get the variables self.dumped_state = False self.long_service_running = True store_dumper = self.cli_motor_set.call_async( - MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=self.DUMPER_POWER) + MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=float(self.DUMPER_POWER)) ) rclpy.spin_until_future_complete(self, store_dumper) if not store_dumper.result().success: @@ -210,6 +218,7 @@ def store_dumper(self) -> bool: # get the variables return False while not self.limitSwitchBottom: if self.cancel_current_srv: + self.get_logger("CHECKING THE LIMIT SWITCH") self.cancel_current_srv = False break time.sleep(0.1) diff --git a/src/motor_control/src/motor_control_node.cpp b/src/motor_control/src/motor_control_node.cpp index 2e679b19..efcfb48e 100644 --- a/src/motor_control/src/motor_control_node.cpp +++ b/src/motor_control/src/motor_control_node.cpp @@ -246,7 +246,12 @@ class MotorControlNode : public rclcpp::Node { // Get the motor controller's current duty cycle command std::optional vesc_get_duty_cycle(uint32_t id) { if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + try{ return this->can_data[id].dutyCycle; + } + catch(...) { + return std::nullopt; + } } else { return std::nullopt; // The data is too stale } @@ -254,7 +259,12 @@ class MotorControlNode : public rclcpp::Node { // Get the current velocity of the motor in RPM (Rotations Per Minute) std::optional vesc_get_velocity(uint32_t id) { if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + try { return this->can_data[id].velocity; + } + catch(...) { + return std::nullopt; + } } else { return std::nullopt; // The data is too stale } @@ -262,7 +272,15 @@ class MotorControlNode : public rclcpp::Node { // Get the current position (tachometer reading) of the motor std::optional vesc_get_position(uint32_t id) { if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { - return (static_cast(this->can_data[id].tachometer) / static_cast(this->pid_controllers[id]->getCountsPerRevolution())) * 360.0; + try{ + float n = static_cast(this->can_data[id].tachometer); + + float d = 14.0 * 360.0; + return (n/ d); + } + catch(...) { + return std::nullopt; + } } else { return std::nullopt; // The data is too stale } @@ -270,7 +288,12 @@ class MotorControlNode : public rclcpp::Node { // Get the current draw of the motor in amps std::optional vesc_get_current(uint32_t id) { if (std::chrono::steady_clock::now() - this->can_data[id].timestamp < this->threshold) { + try{ return this->can_data[id].current; + } + catch(...) { + return std::nullopt; + } } else { return std::nullopt; // The data is too stale } @@ -410,17 +433,12 @@ class MotorControlNode : public rclcpp::Node { void get_callback(const std::shared_ptr request, std::shared_ptr response) { std::optional data = std::nullopt; - - // RCLCPP_INFO(this->get_logger(), "GET COMMAND IS CALLED"); - if (strcmp(request->type.c_str(), "velocity") == 0) { data = vesc_get_velocity(request->can_id); } else if (strcmp(request->type.c_str(), "duty_cycle") == 0) { data = vesc_get_duty_cycle(request->can_id); } else if (strcmp(request->type.c_str(), "position") == 0) { - // RCLCPP_INFO(this->get_logger(), "USING POSITION CALLBACK"); data = vesc_get_position(request->can_id); - // RCLCPP_INFO(this->get_logger(), "looked up data succcessfuly"); } else if (strcmp(request->type.c_str(), "current") == 0) { data = vesc_get_current(request->can_id); } else { diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 1051d098..19e0357a 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -35,7 +35,7 @@ from rovr_interfaces.msg import StreamDeckState # Import custom ROS 2 interfaces -from rovr_interfaces.srv import SetPower, SetScrewMotorSpeed +from rovr_interfaces.srv import SetPower, SetScrewMotorSpeed, SetExtension # Uncomment the line below to use the Xbox controller mappings instead # from rovr_control import xbox_controller_constants as bindings @@ -93,7 +93,7 @@ def __init__(self) -> None: "tilt_digging_start_position", 125.0 ) # Measured in encoder counts self.declare_parameter( - "fast_screw_speed", 4000 + "fast_screw_speed", 20000 ) # Measured in potentiometer units (0 to 1023) self.declare_parameter("DIGGER_SAFETY_ZONE", 120) @@ -159,9 +159,8 @@ def __init__(self) -> None: self.cli_dumper_toggle = self.create_client(Trigger, "dumper/toggle") self.cli_dumper_setPower = self.create_client(SetPower, "dumper/setPower") self.cli_dumper_stop = self.create_client(Trigger, "dumper/stop") - - self.cli_dumper_dumpDumper = self.create_client(Trigger, "dump/dumpDumper") - self.cli_dumper_storeDumper = self.create_client(Trigger, "dump/storeDumper") + self.cli_dumper_dump = self.create_client(Trigger, "dumper/dumpDumper") + self.cli_dumper_store = self.create_client(Trigger, "dumper/storeDumper") # self.cli_digger_toggle = self.create_client(SetPower, "digger/toggle") self.cli_auger_stop = self.create_client(Trigger, "auger/control/stop_all") self.cli_auger_extend = self.create_client( @@ -176,6 +175,21 @@ def __init__(self) -> None: self.cli_screw_start = self.create_client( SetScrewMotorSpeed, "auger/screw/run" ) + self.cli_plunge_extend = self.create_client( + Trigger, "auger/push_motor/extend" + ) + self.cli_plunge_retract = self.create_client( + Trigger, "auger/push_motor/retract" + ) + self.cli_plunge_stop = self.create_client( + Trigger, "auger/push_motor/stop" + ) + self.cli_linear_actuator_tilt = self.create_client( + SetExtension, "auger/tilt_actuator/setExtension" + ) + self.cli_linear_actuator_stop = self.create_client( + Trigger, "auger/tilt_actuator/stop" + ) self.cli_big_agitator_on_off = self.create_client( SetBool, "big_agitator_on_off" ) @@ -386,7 +400,7 @@ async def joystick_callback(self, msg: Joy) -> None: # Check if the digger button is pressed # if msg.buttons[bindings.X_BUTTON] == 1 and buttons[bindings.X_BUTTON] == 0: self.cli_screw_start.call_async( - SetScrewMotorSpeed.Request(speed=self.screw_speed) + SetScrewMotorSpeed.Request(speed=float(self.screw_speed)) ) if msg.buttons[bindings.X_BUTTON] == 0 and buttons[bindings.X_BUTTON] == 1: @@ -394,45 +408,37 @@ async def joystick_callback(self, msg: Joy) -> None: # Check if the dumper button is pressed # if msg.buttons[bindings.B_BUTTON] == 1 and buttons[bindings.B_BUTTON] == 0: - self.cli_dumper_stop.call_async( - Trigger.Request() - ) # Stop whatever the dumper is doing - # Toggle the dumper (extended or retracted) - self.cli_dumper_toggle.call_async(Trigger.Request()) - + self.cli_plunge_extend.call_async(Trigger.Request()) + elif msg.buttons[bindings.B_BUTTON] == 0 and buttons[bindings.B_BUTTON] == 1: + self.cli_plunge_stop.call_async(Trigger.Request()) # Check if the agitator button is pressed # - if msg.buttons[bindings.Y_BUTTON] == 1 and buttons[bindings.Y_BUTTON] == 0: - self.cli_big_agitator_toggle.call_async( - Trigger.Request() - ) # Toggle the agitator motor - # self.cli_small_agitator_toggle.call_async(Trigger.Request()) - # # Toggle the agitator motor - + elif msg.buttons[bindings.Y_BUTTON] == 1 and buttons[bindings.Y_BUTTON] == 0: + self.cli_plunge_retract.call_async(Trigger.Request()) + elif msg.buttons[bindings.Y_BUTTON] == 1 and buttons[bindings.Y_BUTTON] == 0: + self.cli_plunge_stop.call_async(Trigger.Request()) # Manually adjust the dumper position with the left and right # bumpers if ( msg.buttons[bindings.RIGHT_BUMPER] == 1 and buttons[bindings.RIGHT_BUMPER] == 0 ): - self.cli_dumper_dumpDumper.call_async( - Trigger.Request() - ) + self.cli_dumper_dump.call_async(Trigger.Request()) elif ( msg.buttons[bindings.RIGHT_BUMPER] == 0 and buttons[bindings.RIGHT_BUMPER] == 1 ): + self.get_logger().info("Right bumper releases") self.cli_dumper_stop.call_async(Trigger.Request()) elif ( msg.buttons[bindings.LEFT_BUMPER] == 1 and buttons[bindings.LEFT_BUMPER] == 0 ): - self.cli_dumper_storeDumper.call_async( - Trigger.Request() - ) + self.cli_dumper_store.call_async(Trigger.Request()) elif ( msg.buttons[bindings.LEFT_BUMPER] == 0 and buttons[bindings.LEFT_BUMPER] == 1 ): + self.get_logger().info("Left bumper released") self.cli_dumper_stop.call_async(Trigger.Request()) # Manually adjust the height of the digger with the left and right @@ -441,23 +447,24 @@ async def joystick_callback(self, msg: Joy) -> None: msg.buttons[bindings.LEFT_TRIGGER] == 1 and buttons[bindings.LEFT_TRIGGER] == 0 ): - self.cli_auger_extend.call_async(Trigger.Request()) + self.cli_linear_actuator_tilt.call_async(SetExtension.Request(extension=False)) elif ( msg.buttons[bindings.LEFT_TRIGGER] == 0 and buttons[bindings.LEFT_TRIGGER] == 1 ): - self.get_logger().info("In main control node stop") - self.cli_auger_stop.call_async(Trigger.Request()) + self.get_logger().info("Left Trigger released") + self.cli_linear_actuator_stop.call_async(Trigger.Request()) elif ( msg.buttons[bindings.RIGHT_TRIGGER] == 1 and buttons[bindings.RIGHT_TRIGGER] == 0 ): - self.cli_auger_retract.call_async(Trigger.Request()) + self.cli_linear_actuator_tilt.call_async(SetExtension.Request(extension=True)) elif ( msg.buttons[bindings.RIGHT_TRIGGER] == 0 and buttons[bindings.RIGHT_TRIGGER] == 1 ): - self.cli_auger_stop.call_async(Trigger.Request()) + self.get_logger().info("Right trigger released") + self.cli_linear_actuator_stop.call_async(Trigger.Request()) # THE CONTROLS BELOW ALWAYS WORK # diff --git a/src/rovr_control/rovr_control/xbox_controller_constants.py b/src/rovr_control/rovr_control/xbox_controller_constants.py index 1e05f649..08577bf8 100644 --- a/src/rovr_control/rovr_control/xbox_controller_constants.py +++ b/src/rovr_control/rovr_control/xbox_controller_constants.py @@ -1,8 +1,8 @@ # Define all the buttons here X_BUTTON = 2 # toggle screw A_BUTTON = 0 # auto_dig_nav_offload -B_BUTTON = 1 # toggle dumper -Y_BUTTON = 3 # toggle big agitator +B_BUTTON = 1 # plunge extend +Y_BUTTON = 3 # plunge retract LEFT_BUMPER = 4 # dumper retract RIGHT_BUMPER = 5 # dumper extend BACK_BUTTON = 6 # auto_offload @@ -16,7 +16,7 @@ LEFT_JOYSTICK_VERTICAL_AXIS = 1 # FREE RIGHT_JOYSTICK_HORIZONTAL_AXIS = 3 # FREE RIGHT_JOYSTICK_VERTICAL_AXIS = 4 # drive -LEFT_TRIGGER_AXIS = 2 # extend auger -RIGHT_TRIGGER_AXIS = 5 # retract auger +LEFT_TRIGGER_AXIS = 2 # retract linear actuator +RIGHT_TRIGGER_AXIS = 5 # extend linear actuator DPAD_HORIZONTAL_AXIS = 6 # DUMP DPAD_VERTICAL_AXIS = 7 # FREE From 220eb4310c9eb9fb1c46698b5d945bc706c730e9 Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Tue, 28 Apr 2026 16:33:57 -0500 Subject: [PATCH 20/21] removed safety features for PoL --- src/auger/auger/auger_node.py | 210 ++++++++++--------------------- src/dumper/dumper/dumper_node.py | 68 +++------- 2 files changed, 88 insertions(+), 190 deletions(-) diff --git a/src/auger/auger/auger_node.py b/src/auger/auger/auger_node.py index 201a0e10..59936260 100644 --- a/src/auger/auger/auger_node.py +++ b/src/auger/auger/auger_node.py @@ -76,6 +76,7 @@ def __init__(self): self.declare_parameter("SPIN_MOTOR_ID", 0) self.declare_parameter("FAST_SCREW_SPEED", 20000) self.declare_parameter("SLOW_SCREW_SPEED", 0) + self.declare_parameter("LINEAR_ACTUATOR_CURRENT", 0.0) # Local variables here self.MIN_SCREW_DIG_SPEED = self.get_parameter("MIN_SCREW_DIG_SPEED").value @@ -87,6 +88,7 @@ def __init__(self): self.extension_limit_switch = self.get_parameter("extension_limit_switch").value self.auger_stowed = self.get_parameter("AUGER_STOWED").value self.dumper_stowed = self.get_parameter("DUMPER_STOWED").value + self.linear_actuator_current = self.get_parameter("LINEAR_ACTUATOR_CURRENT").value self.TILT_ACTUATOR_CURRENT_THRESHOLD = self.get_parameter( "TILT_ACTUATOR_CURRENT_THRESHOLD" ).value @@ -215,6 +217,10 @@ def __init__(self): self.dumper_stowed_sub = self.create_subscription( Bool, "dumper_stowed", self.dumper_stowed_callback, 10 ) + + self.tilt_linear_actuator_current = self.create_subscription( + Float32, "TiltActuatorCurrent", self.tilt_actuator_current_callback, 10 + ) # TODO Define publishers here self.auger_stowed_pub = self.create_publisher(Bool, "auger_stowed", 10) @@ -231,6 +237,7 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: Caller is responsible for timeouts. """ self.linear_actuator_running = True + lastPowerTime = time.time() # push_motor_pos_future = self.cli_motor_get.call_async( # MotorCommandGet.Request(type="position", can_id=self.PUSH_MOTOR_ID) # ) @@ -256,20 +263,12 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: speed = self.TILT_ACTUATOR_SPEED * (1 if tilt else -1) - motor_set_future = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", can_id=self.TILT_ACTUATOR_ID, value=float(speed) ) ) - - rclpy.spin_until_future_complete(self, motor_set_future) - if not motor_set_future.result().success: - self.get_logger().info("WARNING: Failed to set tilt motor velocity") - self.linear_actuator_running = False - return False - - # gets motor current until it is 0 which means it has hit an limit - # switch + self.get_logger().info("set the duty cycle") time.sleep(1.5) if tilt: self.auger_stowed = False @@ -277,42 +276,24 @@ def set_actuator_tilt_extension(self, tilt: bool) -> bool: msg.data = self.auger_stowed self.auger_stowed_pub.publish(msg) - while True: - motor_get_future = self.cli_motor_get.call_async( - MotorCommandGet.Request( - type="current", - can_id=self.TILT_ACTUATOR_ID, - ) - ) - rclpy.spin_until_future_complete(self, motor_get_future) - if motor_get_future.result().success: - self.get_logger().info(f"Actuator current: {motor_get_future.result().data}") - if ( - abs(motor_get_future.result().data) - < self.TILT_ACTUATOR_CURRENT_THRESHOLD - ): - self.get_logger().info(f"Exiting because current read {motor_get_future.result().data}") - break - else: - self.get_logger().info("WARNING: Failed to read tilt actuator position") - - time.sleep(0.1) - + while True:#time.time() - lastPowerTime < 0.5: if self.cancel_linear_actuator: - self.get_logger().info("Cancelling the linear actuator") + self.cancel_linear_actuator = False break + #TODO figure out current publisher + # if not self.linear_actuator_current < self.current_threshold: + # lastPowerTime = time.time() + time.sleep(0.1) + + - motor_set_future_stop = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", can_id=self.TILT_ACTUATOR_ID, value=0.0 ) ) - rclpy.spin_until_future_complete(self, motor_set_future_stop) self.linear_actuator_running = False - if not motor_set_future_stop.result().success: - self.get_logger().info("WARNING: Failed to stop tilt motor") - return False if not tilt: self.auger_stowed = True self.get_logger().info("Set auger_stowed") @@ -332,7 +313,6 @@ def stop_actuator_tilt(self) -> bool: self.cancel_linear_actuator = True return True - self.get_logger().info("Stopping tilt actuator") motor_set_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", @@ -340,8 +320,7 @@ def stop_actuator_tilt(self) -> bool: value=0.0, ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - return motor_set_future.result().success + return True def set_motor_push_extend(self) -> bool: """ @@ -381,23 +360,23 @@ def set_motor_push_extend(self) -> bool: # self.motor_push_running = False # return False - if ( - self.MAX_PUSH_MOTOR_POSITION > self.MAX_PUSH_MOTOR_POSITION - or self.MAX_PUSH_MOTOR_POSITION < self.MIN_PUSH_MOTOR_POSITION - ): - self.get_logger().warn( - f"WARNING: Requested push motor position is out of range, clamping value; requested: {self.MAX_PUSH_MOTOR_POSITION}" - ) - self.MAX_PUSH_MOTOR_POSITION = max( - self.MIN_PUSH_MOTOR_POSITION, - min(self.MAX_PUSH_MOTOR_POSITION, self.MAX_PUSH_MOTOR_POSITION), - ) # clamp the value to be within range - self.get_logger().info( - "Setting auger push motor position to: " + str(self.MAX_PUSH_MOTOR_POSITION) - ) + # if ( + # self.MAX_PUSH_MOTOR_POSITION > self.MAX_PUSH_MOTOR_POSITION + # or self.MAX_PUSH_MOTOR_POSITION < self.MIN_PUSH_MOTOR_POSITION + # ): + # self.get_logger().warn( + # f"WARNING: Requested push motor position is out of range, clamping value; requested: {self.MAX_PUSH_MOTOR_POSITION}" + # ) + # self.MAX_PUSH_MOTOR_POSITION = max( + # self.MIN_PUSH_MOTOR_POSITION, + # min(self.MAX_PUSH_MOTOR_POSITION, self.MAX_PUSH_MOTOR_POSITION), + # ) # clamp the value to be within range + # self.get_logger().info( + # "Setting auger push motor position to: " + str(self.MAX_PUSH_MOTOR_POSITION) + # ) self.get_logger().info("Setting the plunge motor velocity") - motor_set_future = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( type="velocity", can_id=self.PUSH_MOTOR_ID, @@ -407,16 +386,9 @@ def set_motor_push_extend(self) -> bool: self.get_logger().info( f"Set the plunge velocity to {self.DEFAULT_PUSH_MOTOR_SPEED}" ) - rclpy.spin_until_future_complete(self, motor_set_future) - - if not motor_set_future.result().success: - self.get_logger().warn("WARNING: Failed to set push motor voltage") - self.motor_push_running = False - return False # wait till motor reaches desired position while True: - self.get_logger().info(f"waiting for cancel motor push, cancel_motor_push is {self.cancel_motor_push}") if self.cancel_motor_push: break # self.get_logger().info("Getting the plunge motor position") @@ -443,23 +415,18 @@ def set_motor_push_extend(self) -> bool: # self.get_logger().warn("WARNING: Failed to read push motor position") time.sleep(0.1) + self.get_logger().info("out of the loop") stop_motor_future = self.cli_motor_set.call_async( MotorCommandSet.Request( type="velocity", power_limit=power_limit, can_id=self.PUSH_MOTOR_ID, - value=0, + value=0.0, ) ) self.motor_push_running = False self.cancel_motor_push = False - stop_motor_response = rclpy.spin_until_future_complete(self, stop_motor_future) - if not stop_motor_response.result().success: - self.get_logger().warn("WARNING: Failed to stop the auger screw") - return False - - return True def set_motor_push_retract(self) -> bool: @@ -471,100 +438,55 @@ def set_motor_push_retract(self) -> bool: """ self.motor_push_running = True power_limit = 0.5 - - if ( - self.MIN_PUSH_MOTOR_POSITION > self.MIN_PUSH_MOTOR_POSITION - or self.MIN_PUSH_MOTOR_POSITION < self.MIN_PUSH_MOTOR_POSITION - ): - self.get_logger().warn( - f"WARNING: Requested push motor position is out of range, clamping value; requested: {self.MIN_PUSH_MOTOR_POSITION}" - ) - self.MIN_PUSH_MOTOR_POSITION = max( - self.MIN_PUSH_MOTOR_POSITION, - min(self.MIN_PUSH_MOTOR_POSITION, self.MIN_PUSH_MOTOR_POSITION), - ) # clamp the value to be within range - self.get_logger().info( - "Setting auger push motor position to: " + str(self.MIN_PUSH_MOTOR_POSITION) - ) - - motor_set_future = self.cli_motor_set.call_async( - MotorCommandSet.Request( - type="velocity", - power_limit=power_limit, - can_id=self.PUSH_MOTOR_ID, - value=float(-self.DEFAULT_PUSH_MOTOR_SPEED), + if not self.extension_limit_switch: + motor_set_future = self.cli_motor_set.call_async( + MotorCommandSet.Request( + type="velocity", + power_limit=power_limit, + can_id=self.PUSH_MOTOR_ID, + value=float(-self.DEFAULT_PUSH_MOTOR_SPEED), + ) ) - ) - rclpy.spin_until_future_complete(self, motor_set_future) - - if not motor_set_future.result().success: - self.get_logger().warn("WARNING: Failed to set push motor voltage") - self.motor_push_running = False - return False - - # wait till motor reaches desired position - while True: + + while not self.extension_limit_switch: if self.cancel_motor_push: break - motor_get_pos_future = self.cli_motor_get.call_async( - MotorCommandGet.Request( - type="position", - can_id=self.PUSH_MOTOR_ID, - ) - ) - rclpy.spin_until_future_complete(self, motor_get_pos_future) - - if motor_get_pos_future.result().success: - current_pos = motor_get_pos_future.result().data - msg = Float32() - msg.data = current_pos - self.extension_pos_pub.publish(msg) - if ( - (-self.DEFAULT_PUSH_MOTOR_SPEED <= 0 and current_pos <= self.MIN_PUSH_MOTOR_POSITION) - or (-self.DEFAULT_PUSH_MOTOR_SPEED > 0 and current_pos >= self.MAX_PUSH_MOTOR_POSITION) - or (self.extension_limit_switch) - ): - break - else: - self.get_logger().warn("WARNING: Failed to read push motor position") + + # current_pos = motor_get_pos_future.result().data + # msg = Float32() + # msg.data = current_pos + # self.extension_pos_pub.publish(msg) time.sleep(0.1) - stop_motor_future = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( type="velocity", power_limit=power_limit, can_id=self.PUSH_MOTOR_ID, - value=0, + value=0.0, ) ) self.motor_push_running = False self.cancel_motor_push = False - stop_motor_response = rclpy.spin_until_future_complete(self, stop_motor_future) - if not stop_motor_response.result().success: - self.get_logger().warn("WARNING: Failed to stop the auger screw") - return False - return True def stop_motor_push(self) -> bool: """Stop the motor that pushes the auger into the ground.""" - self.get_logger().info("In the stop acutator tilt function") if self.motor_push_running: self.cancel_motor_push = True - self.get_logger.info(f"cancel_motor_push is {self.cancel_motor_push}") + self.get_logger().info(f"cancel_motor_push is {self.cancel_motor_push}") return True - - motor_set_future = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", can_id=self.PUSH_MOTOR_ID, value=0.0, ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - return motor_set_future.result().success + self.get_logger().info("stopping plunge") + return True def run_auger_spin_velocity(self, desired_speed: float, power_limit: float) -> bool: """Set the auger spin velocity of the auger motor.""" @@ -591,22 +513,21 @@ def run_auger_spin_velocity(self, desired_speed: float, power_limit: float) -> b power_limit=float(power_limit), ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - return motor_set_future.result().success + return True def stop_auger_spin(self) -> bool: """Stop the auger motor from spinning.""" self.get_logger().info("Stopping auger spin") - motor_set_future = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request( type="duty_cycle", can_id=self.SPIN_MOTOR_ID, value=0.0, ) ) - rclpy.spin_until_future_complete(self, motor_set_future) - return motor_set_future.result().success + + return True def extend_digger(self) -> bool: """Tilt and extend""" @@ -698,6 +619,9 @@ def limit_switch_callback(self, msg): def dumper_stowed_callback(self, msg): self.dumper_stowed = msg.data + + def tilt_actuator_current_callback(self, msg): + self.linear_actuator_current = msg.data def extend_push_callback(self, request, response): """ @@ -737,9 +661,11 @@ def main(args=None): rclpy.init(args=args) node = Auger() - + executor = MultiThreadedExecutor() + executor.add_node(node + ) node.get_logger().info("Initializing the Auger subsystem!") - rclpy.spin(node) + executor.spin() node.get_logger().info(" subsystem!") diff --git a/src/dumper/dumper/dumper_node.py b/src/dumper/dumper/dumper_node.py index ba8b66cc..ce6783de 100644 --- a/src/dumper/dumper/dumper_node.py +++ b/src/dumper/dumper/dumper_node.py @@ -79,11 +79,8 @@ def __init__(self): # Dumper Current Threshold self.current_threshold = 0.3 - self.dumper_current = 0.0 - self.dumper_current_sub = self.create_subscription( - Float32, "Dumper_Current", self.dumper_current_callback, 10 - ) + self.KillSwitch_sub = self.create_subscription( Bool, "DumperLimitSwitch", self.killSwitch_callback, 10 @@ -110,15 +107,11 @@ def set_power(self, dumper_power: float) -> None: def stop(self) -> bool: """This method stops the dumper.""" - stop_dumper = self.cli_motor_set.call_async( + self.cli_motor_set.call_async( MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=float(0.0)) ) self.get_logger().info("Waiting to stop the dumper...") - rclpy.spin_until_future_complete(self, stop_dumper) - self.get_logger().info("Done waiting to stop the dumper!") - if not stop_dumper.result().success: - self.get_logger().error("Failed to stop the dumper") - return stop_dumper.result().success + return True def toggle(self) -> None: """This method toggles the dumper.""" @@ -140,7 +133,6 @@ def stop_callback(self, request, response): if self.long_service_running: self.cancel_current_srv = True self.get_logger().info(f"inside if statement, cancel_current_srv: {self.cancel_current_srv}") - return True self.stop() response.success = True @@ -176,22 +168,8 @@ def dump_dumper(self) -> None: if self.cancel_current_srv: self.get_logger().info("cancel current srv is true") break - motor_get_future = self.cli_motor_get.call_async( - MotorCommandGet.Request( - type="position", - can_id=self.DUMPER_MOTOR, - ) - ) - rclpy.spin_until_future_complete(self, motor_get_future) - if motor_get_future.result().success: - if ( - abs(motor_get_future.result().data - self.DUMP_POS) < 5 - ): - break - else: - self.get_logger().info("WARNING: Failed to read tilt actuator position") - time.sleep(0.1) + self.get_logger().info("Finished dump dumper service") self.stop() self.long_service_running = False @@ -199,43 +177,37 @@ def dump_dumper(self) -> None: self.get_logger().info("Done dumping the dumper") def dump_callback(self, request, response): - return self.dump_dumper() + self.dump_dumper() + response.success = True + return response def store_dumper(self) -> bool: # get the variables if not self.auger_stowed: self.get_logger().info("The Auger is already extended") return self.get_logger().info("Retracting the dumper") - self.dumped_state = False + self.long_service_running = True - store_dumper = self.cli_motor_set.call_async( - MotorCommandSet.Request(type="duty_cycle", can_id=self.DUMPER_MOTOR, value=float(self.DUMPER_POWER)) - ) - rclpy.spin_until_future_complete(self, store_dumper) - if not store_dumper.result().success: - self.get_logger().error("Failed to start storing the dumper") - self.long_service_running = False - return False + if not self.limitSwitchBottom: + store_dumper = self.cli_motor_set.call_async( + MotorCommandSet.Request(type="velocity", can_id=self.DUMPER_MOTOR, value=float(-self.DUMPER_VEL)) + ) + while not self.limitSwitchBottom: + self.get_logger().info(f"Dumper limit switch is {self.limitSwitchBottom}") if self.cancel_current_srv: - self.get_logger("CHECKING THE LIMIT SWITCH") - self.cancel_current_srv = False break time.sleep(0.1) - time.sleep(2.0) - - stop_success = self.stop() - if not stop_success: - return False - self.stop() self.dumper_stowed = True msg = Bool() msg.data = self.dumper_stowed self.dumper_stowed_pub.publish(msg) self.long_service_running = False + self.cancel_current_srv = False self.get_logger().info("Done storing the dumper") + self.dumped_state = False return True @@ -243,14 +215,14 @@ def store_dumper(self) -> bool: # get the variables # the storage bin can only be dumped back # when the auger is completely stowed (both actuators fully stored) def store_callback(self, request, response): - return self.store_dumper() + self.store_dumper() + response.success = True + return response - def dumper_current_callback(self, msg): - self.dumper_current = msg.data def killSwitch_callback(self, msg): # position control... - self.LimitSwitchBottom = msg.data + self.limitSwitchBottom = msg.data def auger_stowed_callback(self, msg): self.auger_stowed = msg.data From 0433f3dc12c916de1a37fcf7177c8570ff951d49 Mon Sep 17 00:00:00 2001 From: umnrobotics Date: Tue, 28 Apr 2026 17:59:22 -0500 Subject: [PATCH 21/21] more changes before outside --- scripts/can_reset.sh | 5 +++++ scripts/joy_reset.sh | 5 +++++ .../rovr_control/main_control_node.py | 17 ++++++++++++----- 3 files changed, 22 insertions(+), 5 deletions(-) create mode 100755 scripts/can_reset.sh create mode 100755 scripts/joy_reset.sh diff --git a/scripts/can_reset.sh b/scripts/can_reset.sh new file mode 100755 index 00000000..f7a3edab --- /dev/null +++ b/scripts/can_reset.sh @@ -0,0 +1,5 @@ +#!/bin/bash +sudo ip link set can0 down +sudo ip link set can0 type can restart-ms 100 +sudo ip link set can0 up +candump can0 \ No newline at end of file diff --git a/scripts/joy_reset.sh b/scripts/joy_reset.sh new file mode 100755 index 00000000..326677e9 --- /dev/null +++ b/scripts/joy_reset.sh @@ -0,0 +1,5 @@ +#!/bin/bash +source /opt/ros/humble/setup.bash +ros2 daemon stop +ros2 daemon start +ros2 topic echo joy \ No newline at end of file diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 19e0357a..2f8cad0f 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -95,6 +95,8 @@ def __init__(self) -> None: self.declare_parameter( "fast_screw_speed", 20000 ) + self.screwing = False + # Measured in potentiometer units (0 to 1023) self.declare_parameter("DIGGER_SAFETY_ZONE", 120) # The power the dumper needs to go @@ -399,12 +401,17 @@ async def joystick_callback(self, msg: Joy) -> None: # Check if the digger button is pressed # if msg.buttons[bindings.X_BUTTON] == 1 and buttons[bindings.X_BUTTON] == 0: - self.cli_screw_start.call_async( - SetScrewMotorSpeed.Request(speed=float(self.screw_speed)) - ) + if self.screwing: + self.cli_screw_stop.call_async(Trigger.Request()) + self.screwing = False + elif not self.screwing: + self.cli_screw_start.call_async( + SetScrewMotorSpeed.Request(speed=float(self.screw_speed)) + ) + self.screwing=True - if msg.buttons[bindings.X_BUTTON] == 0 and buttons[bindings.X_BUTTON] == 1: - self.cli_screw_stop.call_async(Trigger.Request()) + # if msg.buttons[bindings.X_BUTTON] == 0 and buttons[bindings.X_BUTTON] == 1: + # self.cli_screw_stop.call_async(Trigger.Request()) # Check if the dumper button is pressed # if msg.buttons[bindings.B_BUTTON] == 1 and buttons[bindings.B_BUTTON] == 0: