Add IR to camera driver

cam_driver/scripts/cam_driver

@@ -14,8 +14,10 @@ class CameraDriver(Node):
         super().__init__(name) # Create the node
 
         # Initialize publishers
-        self._rgb_publisher = self.create_publisher(Image, 'rgb_cam', 10)
-        self._depth_publisher = self.create_publisher(Image, 'depth_cam', 10)
+        self._rgb_publisher = self.create_publisher(Image, 'cam_rgb', 10)
+        self._depth_publisher = self.create_publisher(Image, 'cam_depth', 10)
+        self._infra_publisher_1 = self.create_publisher(Image, 'cam_infra_1',10)
+        self._infra_publisher_2 = self.create_publisher(Image, 'cam_infra_2',10)
 
         # Initialize a clock for the publisher
         self.create_timer(timerFreq, self.publish_imgs)
@@ -47,39 +49,65 @@ class CameraDriver(Node):
         ## Configure stream width, height, format and frequency
         self._config.enable_stream(rs.stream.color, width=848, height=480, format=rs.format.bgr8, framerate=60)
         self._config.enable_stream(rs.stream.depth, width=848, height=480, format=rs.format.z16, framerate=60)
+        self._config.enable_stream(rs.stream.infrared, 1, width=848, height=480, format=rs.format.y8, framerate=60)
+        self._config.enable_stream(rs.stream.infrared, 2, width=848, height=480, format=rs.format.y8, framerate=60)
 
         ## Start the acquisition    
         self._pipeline.start(self._config)
 
     def read_imgs(self):
         """lire et traduire images camera"""
+        
+        # Get frames
         frames = self._pipeline.wait_for_frames()
+        
+        # Split frames
         color_frame = frames.first(rs.stream.color)
         depth_frame = frames.first(rs.stream.depth)
-        if not (depth_frame and color_frame):
+        infra_frame_1 = frames.get_infrared_frame(1)
+        infra_frame_2 = frames.get_infrared_frame(2)
+
+
+        if not (depth_frame and color_frame and infra_frame_1 and infra_frame_2):
+            print("eh oh le truc y marche pas")
             return
         
         # Convert images to numpy arrays
         depth_image = np.asanyarray(depth_frame.get_data())
         color_image = np.asanyarray(color_frame.get_data())
+        infra_image_1 = np.asanyarray(infra_frame_1.get_data())
+        infra_image_2 = np.asanyarray(infra_frame_2.get_data())
 
-        # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
+        # Apply colormap on depth and IR images (image must be converted to 8-bit per pixel first)
         depth_colormap   = cv2.applyColorMap(cv2.convertScaleAbs(depth_image,   alpha=0.03), cv2.COLORMAP_JET)
+        infra_colormap_1 = cv2.applyColorMap(cv2.convertScaleAbs(infra_image_1, alpha=1), cv2.COLORMAP_JET)
+        infra_colormap_2 = cv2.applyColorMap(cv2.convertScaleAbs(infra_image_2, alpha=1), cv2.COLORMAP_JET)
         
         bridge=CvBridge()
-
+        # Convert to ROS2 Image type
+        # - RGB Image
         self._rgb_image = bridge.cv2_to_imgmsg(color_image,"bgr8")
-        self._depth_image = bridge.cv2_to_imgmsg(depth_colormap, "bgr8")
         self._rgb_image.header.stamp = self.get_clock().now().to_msg()
-        self._depth_image.header.stamp = self._rgb_image.header.stamp
         self._rgb_image.header.frame_id = "image"
+        # - Depth Image
+        self._depth_image = bridge.cv2_to_imgmsg(depth_colormap, "bgr8")
+        self._depth_image.header.stamp = self._rgb_image.header.stamp
         self._depth_image.header.frame_id = "depth"
+        # - IR1 Image
+        self._infra1_image = bridge.cv2_to_imgmsg(infra_colormap_1,"bgr8")
+        self._infra1_image.header.stamp = self._rgb_image.header.stamp
+        self._infra1_image.header.frame_id = "infrared_1"
+        # - IR2 Image
+        self._infra2_image = bridge.cv2_to_imgmsg(infra_colormap_2,"bgr8")
+        self._infra2_image.header.stamp = self._rgb_image.header.stamp
+        self._infra2_image.header.frame_id = "infrared_2"
 
-        return self._rgb_image, self._depth_image
+        return self._rgb_image, self._depth_image, self._infra1_image, self._infra2_image
 
     def publish_imgs(self):
         self.publish_rgb()
         self.publish_depth()
+        self.publish_IR()
 
 
     def publish_rgb(self):
@@ -100,6 +128,16 @@ class CameraDriver(Node):
 
         return 0
 
+    def publish_IR(self):
+        """ 
+        Publish infrared colormap image
+        """
+
+        self._infra_publisher_1.publish(self._infra1_image)
+        self._infra_publisher_2.publish(self._infra2_image)
+
+        return 0
+
 # Capture ctrl-c event
 isOk=True
 def signalInterruption(signum, frame):