add camera launch

Author: ingibirka

src/stingray_core_sensors/CMakeLists.txt

@@ -22,4 +22,9 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_package()

src/stingray_core_sensors/camera.yaml

@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: front_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [447.3296, 0.000000, 326.9216, 0.000000, 600.2483, 259.8586, 0.000000, 0.000000, 1.000000]
+distortion_model: radtan
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.36871094084873407, 0.13770812146992423, -0.002057012920641659, 0.0022137169808520724, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [447.3296, 0.000000, 326.9216, 0.000000, 0.000000, 600.2483, 259.8586, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

src/stingray_core_sensors/launch/video_display.launch.py

@@ -0,0 +1,83 @@
+from pathlib import Path
+
+from ament_index_python import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    try:
+        calibration_config_dir = Path(get_package_share_directory('stingray_core_sensors')) / "configs"
+        camera_calibration_path = calibration_config_dir / "camera.yaml"
+    except Exception:
+        camera_calibration_path = ""
+
+    camera_topic_arg_1 = DeclareLaunchArgument(
+        "camera_topic_1", 
+        default_value='/stingray_core/topics/camera_1'
+    )
+    camera_topic_arg_2 = DeclareLaunchArgument(
+        "camera_topic_2", 
+        default_value='/stingray_core/topics/camera_2'
+    )
+    camera_path_arg_1 = DeclareLaunchArgument(
+        "camera_path_1", 
+        default_value='/dev/video0'
+    )
+    camera_path_arg_2 = DeclareLaunchArgument(
+        "camera_path_2", 
+        default_value='/dev/video2'
+    )
+
+
+
+    camera_calibration_path_arg = DeclareLaunchArgument(
+        "camera_calibration_path", default_value=str(camera_calibration_path)
+    )
+ 
+    
+    camera_1_node = Node(
+        package='usb_cam',
+        executable='usb_cam_node_exe',
+        name='video_display_node_1',
+        output='screen',
+        remappings=[
+            ('/image_raw', LaunchConfiguration("camera_topic_1")), 
+        ],
+        parameters=[
+            {'video_device': LaunchConfiguration("camera_path_1")},
+            {'params-file': LaunchConfiguration("camera_calibration_path")},
+        ],
+        respawn=True,
+        respawn_delay=1,
+    )
+
+    camera_2_node = Node(
+        package='usb_cam',
+        executable='usb_cam_node_exe',
+        name='video_display_node_2',
+        output='screen',
+        remappings=[
+            ('/image_raw', LaunchConfiguration("camera_topic_2")), 
+        ],
+        parameters=[
+            {'video_device': LaunchConfiguration("camera_path_2")},
+            {'params-file': LaunchConfiguration("camera_calibration_path")},
+        ],
+        respawn=True,
+        respawn_delay=1,
+    )
+
+
+    return LaunchDescription([
+        camera_topic_arg_1,
+        camera_topic_arg_2,
+        camera_path_arg_1,
+        camera_path_arg_2,
+        camera_calibration_path_arg, 
+        camera_1_node,
+        camera_2_node,
+    ])