cam driver

cam_driver/CMakeLists.txt

+cmake_minimum_required(VERSION 3.8)
+project(tuto_kickoff)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
+# Install launch files.
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+# Python scripts
+install( PROGRAMS 
+  scripts/cam_driver
+  DESTINATION lib/${PROJECT_NAME}
+)
\ No newline at end of file

cam_driver/package.xml

+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cam_driver</name>
+  <version>0.0.0</version>
+  <description>Package de la caméra Intel RealSense</description>
+  <maintainer email="lucas.naury@etu.imt-nord-europe.fr">lucas.naury</maintainer>
+  <maintainer email="ines.el.hadri@etu.imt-nord-europe.fr">ines.el.hadri</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <!--Dependencies-->
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

cam_driver/scripts/cam_driver

+#!/usr/bin/python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+import pyrealsense2 as rs
+import signal, time, numpy as np
+import sys, cv2
+
+
+class CameraDriver(Node):
+
+    def __init__(self, name="cam_driver", timerFreq = 1/60.0):
+        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)
+
+        # Initialize a clock for the publisher
+        self.create_timer(timerFreq, self.publish_velo)
+
+
+        # ------------------------------ Initialize camera ------------------------------ #
+
+        ## Configure depth and color streams
+        self._pipeline = rs.pipeline()
+        self._config = rs.config()
+
+        ## Get device product line for setting a supporting resolution
+        pipeline_wrapper = rs.pipeline_wrapper(self._pipeline)
+        pipeline_profile = self._config.resolve(pipeline_wrapper)
+        device = pipeline_profile.get_device()
+        device_product_line = str(device.get_info(rs.camera_info.product_line))
+
+        print( f"Connect: {device_product_line}" )
+        found_rgb = True
+        for s in device.sensors:
+            print( "Name:" + s.get_info(rs.camera_info.name) )
+            if s.get_info(rs.camera_info.name) == 'RGB Camera':
+                found_rgb = True
+
+        if not (found_rgb):
+            print("Depth camera required !!!")
+            exit(0)
+
+        ## 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)
+        
+        ## Start the acquisition    
+        self._pipeline.start(self._config)
+
+    def read_imgs(self):
+        """lire et traduire images camera"""
+        frames = self._pipeline.wait_for_frames()
+        color_frame = frames.first(rs.stream.color)
+        depth_frame = frames.first(rs.stream.depth)
+        if not (depth_frame and color_frame):
+            return
+        
+        # Convert images to numpy arrays
+        depth_image = np.asanyarray(depth_frame.get_data())
+        color_image = np.asanyarray(color_frame.get_data())
+
+        # Apply colormap on depth image (image must be converted to 8-bit per pixel first)
+        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
+
+        self._rgb_image = color_image
+        self._depth_image = depth_colormap
+        return color_image, depth_colormap
+
+    def publish_imgs(self):
+        self.publish_rgb()
+        self.publish_depth()
+
+
+    def publish_rgb(self):
+        """ 
+        Publish RGB image
+        """
+
+        self._rgb_publisher.publish(self._rgb_image)
+
+        return 0
+    
+    def publish_depth(self):
+        """ 
+        Publish depth colormap image
+        """
+
+        self._rgb_publisher.publish(self._depth_image)
+
+        return 0
+
+# Capture ctrl-c event
+isOk=True
+def signalInterruption(signum, frame):
+    global isOk
+    print( "\nCtrl-c pressed" )
+    isOk= False
+
+def main():
+    """
+    Main loop
+    """
+
+    signal.signal(signal.SIGINT, signalInterruption)
+
+    rclpy.init()
+    rosNode = CameraDriver()
+    isOk = True
+    while isOk:
+        rosNode.read_imgs()
+        rosNode.publish_imgs()
+        rclpy.spin_once(rosNode, timeout_sec=0.001)
+
+
+    rosNode.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file