Camera user interface

README.md

@@ -99,6 +99,8 @@ devcontainer build --push true --workspace-folder . --platform="linux/amd64,linu
 <details>
 <summary>How to Run Inside the ISAAC ROS Container on Linux/Jetson</summary>
 <br>
+** 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

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

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 \

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
 

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/<repo-name>:<image-layers-separated-by->_<HASH>`
 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. 
 

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}
 

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()