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/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/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 \ 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/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. diff --git a/scripts/enter_isaac_ros_container.sh b/scripts/enter_isaac_ros_container.sh index 03185f19..ceaa73f2 100755 --- a/scripts/enter_isaac_ros_container.sh +++ b/scripts/enter_isaac_ros_container.sh @@ -1,7 +1,21 @@ #!/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 \ +-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/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/compression.py b/src/camera_interface/camera_interface/compression.py new file mode 100644 index 00000000..45fe3957 --- /dev/null +++ b/src/camera_interface/camera_interface/compression.py @@ -0,0 +1,161 @@ +#!/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() + 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 + + # 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 + + # Codec Settings + self.codec = 'hevc_nvenc' + self.stream_fmt = 'hevc' + + self.publisher_ = self.create_publisher(CompressedImage, self.topic_name, 10) + + # Start the background thread for FFmpeg output + self.output_thread = threading.Thread(target=self.read_encoded_stream, daemon=True) + self.output_thread.start() + + # Timer for capturing frames + self.input_timer = self.create_timer(1.0 / self.fps, self.feed_encoder) + + 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) + + w = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH) or 640 + h = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT) or 480 + 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 + + return True + + def setup_ffmpeg(self): + """Initializes the FFmpeg subprocess.""" + if self.process: + self.process.kill() + + 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, '-' + ] + + self.process = subprocess.Popen( + ffmpeg_cmd, stdin=subprocess.PIPE, + stdout=subprocess.PIPE, stderr=subprocess.PIPE, bufsize=0 + ) + + def feed_encoder(self): + """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 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: + if self.process and self.process.stdin: + self.process.stdin.write(frame.tobytes()) + self.process.stdin.flush() + 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.""" + chunk_size = 256 * 1024 + 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() + if self.cap: + 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() \ 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 new file mode 100644 index 00000000..887e0d96 --- /dev/null +++ b/src/camera_interface/camera_interface/qt_user_interface.py @@ -0,0 +1,393 @@ +import sys +import threading +import time +import math +import signal +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, QPainter, QColor, QFont +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 -------------------- + +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.last_received_time = time.time() + self.start_time = time.time() # Track when we started waiting + self.is_disconnected = True + + self.label = QLabel("Initializing...") + self.label.setAlignment(Qt.AlignCenter) + self.label.setStyleSheet("background-color: #000; color: #555;") + self.label.setSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) + + 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) + + from PySide6.QtCore import QTimer + 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: + self.show_no_signal() + else: + self.is_disconnected = False + + def show_no_signal(self): + """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()) + + # 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: + # 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 + + painter = QPainter(base_pixmap) + if overlay_color.alpha() > 0: + painter.fillRect(base_pixmap.rect(), overlay_color) + + 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): + self.last_received_time = time.time() + self.is_disconnected = False + self.last_frame = cv_img.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( + self.label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation + )) + + def resizeEvent(self, event): + 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): + 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) + + 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() + + 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) + + if item.checkState() == Qt.Checked: + self.add_camera(topic, msg_type) + else: + self.remove_camera(topic) + + 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.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() + # 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: + app.exec() + except KeyboardInterrupt: + pass + finally: + # 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 diff --git a/src/camera_interface/camera_interface/webcam.py b/src/camera_interface/camera_interface/webcam.py new file mode 100644 index 00000000..7f4ec84c --- /dev/null +++ b/src/camera_interface/camera_interface/webcam.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CompressedImage +import cv2 +import subprocess +import threading +import time + +class GpuWebcamPublisher(Node): + def __init__(self): + 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 + ] + + 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) + + 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 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 = 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 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..1f35afe0 --- /dev/null +++ b/src/camera_interface/launch/camera_system_launch.py @@ -0,0 +1,31 @@ +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/ + # 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"}, + ] + + 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/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..ae92bfe3 --- /dev/null +++ b/src/camera_interface/setup.py @@ -0,0 +1,35 @@ +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']), + ('share/' + package_name + '/launch', + ['launch/camera_system_launch.py']), + ], + 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': [ + 'qt_user_interface = camera_interface.qt_user_interface:main', + 'webcam = camera_interface.webcam:main', + 'webcam_raw = camera_interface.webcam_raw:main', + 'compression = camera_interface.compression: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' 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