changed decompress and added the nodes to the setup

Author: kemary04

src/ptz_cam/ptz_cam/ipcamera_decompress.py

@@ -11,30 +11,121 @@ 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',
         ],
     },
 )