changed the CameraEncoder to see now if the system works

src/ptz_cam/ptz_cam/ipcamera_compress.py

@@ -20,30 +20,36 @@ def __init__(self):
             'arm_rgb': (
                 '/camera/arm/rgb_active',
                 lambda: [
-                    'gst-launch-1.0', 'rs2src', 'device=/dev/video2', 'stream-type=0',
+                    'gst-launch-1.0',
+                    'v4l2src', 'device=/dev/video2',
+                    '!', 'video/x-raw,width=640,height=480,framerate=30/1',
                     '!', 'videoconvert',
-                    '!', 'x264enc', 'tune=zerolatency', 'speed-preset=ultrafast',
-                    '!', 'rtpmp4gpay', 'config-interval=1',
+                    '!', 'x264enc', 'tune=zerolatency', 'speed-preset=ultrafast', 'bitrate=2000',
+                    '!', 'rtph264pay', 'config-interval=1', 'pt=96',
                     '!', 'udpsink', 'host=127.0.0.1', 'port=5002'
                 ]
             ),
             'arm_ir': (
                 '/camera/arm/ir_active',
                 lambda: [
-                    'gst-launch-1.0', 'rs2src', 'device=/dev/video2', 'stream-type=2',
+                    'gst-launch-1.0',
+                    'v4l2src', 'device=/dev/video4',
+                    '!', 'video/x-raw,width=640,height=480,framerate=30/1',
                     '!', 'videoconvert',
-                    '!', 'x264enc', 'tune=zerolatency', 'speed-preset=ultrafast',
-                    '!', 'rtpmp4gpay', 'config-interval=1',
+                    '!', 'x264enc', 'tune=zerolatency', 'speed-preset=ultrafast', 'bitrate=2000',
+                    '!', 'rtph264pay', 'config-interval=1', 'pt=96',
                     '!', 'udpsink', 'host=127.0.0.1', 'port=5003'
                 ]
             ),
             'front_rgb': (
                 '/camera/front/rgb_active',
                 lambda: [
-                    'gst-launch-1.0', 'rs2src', 'device=/dev/video8', 'stream-type=0',
+                    'gst-launch-1.0',
+                    'v4l2src', 'device=/dev/video8',
+                    '!', 'video/x-raw,width=640,height=480,framerate=30/1',
                     '!', 'videoconvert',
-                    '!', 'x264enc', 'tune=zerolatency', 'speed-preset=ultrafast',
-                    '!', 'rtpmp4gpay', 'config-interval=1',
+                    '!', 'x264enc', 'tune=zerolatency', 'speed-preset=ultrafast', 'bitrate=2000',
+                    '!', 'rtph264pay', 'config-interval=1', 'pt=96',
                     '!', 'udpsink', 'host=127.0.0.1', 'port=5004'
                 ]
             ),
@@ -73,10 +79,28 @@ def toggle_camera(self, name, active):
                 self.processes[name].terminate()
                 self.processes[name].wait()
                 del self.processes[name]
+
+    def destroy_node(self):
+        self.get_logger().info("Shutting down, stopping all cameras...")
+
+        for name in list(self.processes.keys()):
+            self.processes[name].terminate()
+            self.processes[name].wait(timeout=5)
+
+        super().destroy_node()
 
-def main():
-    rclpy.init()
+def main(args=None):
+    rclpy.init(args=args)
     node = CameraEncoder()
-    rclpy.spin(node)
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+
     node.destroy_node()
     rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/ptz_cam/ptz_cam/ipcamera_decompress.py

@@ -1,40 +1,130 @@
 import rclpy
 from rclpy.node import Node
 from sensor_msgs.msg import Image
+from std_msgs.msg import Bool
 from cv_bridge import CvBridge
-import subprocess
 import cv2
-import threading
 
 class CameraDecoder(Node):
     def __init__(self):
         super().__init__('camera_decoder')
         self.bridge = CvBridge()
 
-        self.udp_ports = {
-            'wrist': 5000,
-            'arm_rgb': 5002,
-            'arm_ir': 5003,
-            'front_rgb': 5004,
-            'ptz': 5005
+        self.cameras = {
+            'wrist': {
+                'port': 5000,
+                'image_topic': '/camera/wrist/image_raw'
+            },
+            'arm_rgb': {
+                'port': 5002,
+                'image_topic': '/camera/arm/rgb/image_raw'
+            },
+            'arm_ir': {
+                'port': 5003,
+                'image_topic': '/camera/arm/ir/image_raw'
+            },
+            'front_rgb': {
+                'port': 5004,
+                'image_topic': '/camera/front/rgb/image_raw'
+            },
+            'ptz': {
+                'port': 5005,
+                'image_topic': '/camera/ptz/image_raw'
+            }
         }
 
-        for name, port in self.udp_ports.items():
-            threading.Thread(target=self.decode_loop, args=(name, port), daemon=True).start()
+        self.captures = {}
+        self.timers = {}
+        self.publishers = {}
 
-    def decode_loop(self, name, port):
-        cap = cv2.VideoCapture(f'udp://127.0.0.1:{port}', cv2.CAP_FFMPEG)
-        pub = self.create_publisher(Image, f'/camera/{name}/image_raw', 10)
+        for name, config in self.cameras.items():
+            self.publishers[name] = self.create_publisher(
+                Image, 
+                config['image_topic'], 
+                10
+            )
 
-        while rclpy.ok():
-            ret, frame = cap.read()
-            if ret:
-                msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
-                pub.publish(msg)
+        for name in self.cameras.keys():
+            self.toggle_decoder(name, True)
 
-def main():
-    rclpy.init()
+    def toggle_decoder(self, name, active):
+        if active:
+            if name not in self.captures:
+                config = self.cameras[name]
+                udp_url = f'udp://127.0.0.1:{config["port"]}'
+                self.get_logger().info(f"Starting decoder for {name} from {udp_url}")
+
+
+                cap = cv2.VideoCapture(udp_url, cv2.CAP_FFMPEG)
+                cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+
+                if not cap.isOpened():
+                    self.get_logger().error(f"Cannot open UDP stream for {name}")
+                    return
+
+                self.captures[name] = cap
+
+                    # Create timer to decode frames at 30 FPS
+                self.timers[name] = self.create_timer(
+                    0.033,  # ~30 FPS
+                    lambda n=name: self.decode_and_publish(n)
+                )
+
+                self.get_logger().info(f"Successfully started decoder for {name}")
+
+
+            else:
+                self.get_logger().warn(f"Decoder for {name} is already running")
+        else:
+            if name in self.captures:
+                self.get_logger().info(f"Stopping decoder for {name}")
+
+                if name in self.timers:
+                    self.timers[name].cancel()
+                    del self.timers[name]
+
+                self.captures[name].release()
+                del self.captures[name]
+
+                self.get_logger().info(f"Successfully stopped decoder for {name}")
+            else:
+                self.get_logger().warn(f"Decoder for {name} is not running")
+
+    def decode_and_publish(self, name):
+        if name not in self.captures:
+            return
+
+        cap = self.captures[name]
+        ret, frame = cap.read()
+
+        if not ret:
+            return
+
+        try:
+            # Convert to ROS2 Image message
+            img_msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
+            img_msg.header.stamp = self.get_clock().now().to_msg()
+            img_msg.header.frame_id = name
+
+            # Publish
+            self.publishers[name].publish(img_msg)
+
+        except Exception as e:
+            self.get_logger().error(f"Failed to publish frame from {name}: {e}")
+
+
+def main(args=None):
+    rclpy.init(args=args)
     node = CameraDecoder()
-    rclpy.spin(node)
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+
     node.destroy_node()
     rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/ptz_cam/setup.py

@@ -30,6 +30,8 @@
             'ipcamera = ptz_cam.ipcamera:main',
             'ipcamerazoom = ptz_cam.ipcamerazoom:main',
             'pitch_tilt_node = ptz_cam.pitch_tilt_node:main',
+            'ipcamera_compress = ptz_cam.ipcamera_compress:main',
+            'ipcamera_decompress = ptz_cam.ipcamera_decompress:main',
         ],
     },
 )