Add drivers for lidar and camera

docker/interfacing/sensor_interfacing/sensor_interfacing.Dockerfile

@@ -7,7 +7,6 @@ WORKDIR ${AMENT_WS}/src
 
 # Copy in source code 
 COPY src/interfacing/sensor_interfacing sensor_interfacing
-COPY src/wato_msgs/sample_msgs sample_msgs
 
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
@@ -19,6 +18,11 @@ RUN apt-get -qq update && rosdep update && \
 ################################# Dependencies ################################
 FROM ${BASE_IMAGE} as dependencies
 
+# Camera and LiDAR ROS2 Driver
+RUN apt update && apt install -y ros-$ROS_DISTRO-spinnaker-camera-driver ros-$ROS_DISTRO-velodyne
+
+RUN apt install -y ros-$ROS_DISTRO-rviz2 ros-$ROS_DISTRO-image-proc
+
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
 RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
@@ -32,6 +36,10 @@ WORKDIR /
 RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \
     rm -rf /root/* /root/.ros /tmp/* /var/lib/apt/lists/* /usr/share/doc/*
 
+
+# Enable X11 Forwarding
+RUN apt-get install -qqy x11-apps
+
 ################################ Build ################################
 FROM dependencies as build
 

modules/dev_overrides/docker-compose.infrastructure.yaml

@@ -9,4 +9,4 @@ services:
   data_stream:
     extends:
       file: ../docker-compose.infrastructure.yaml
-      service: data_stream
+      service: data_stream

modules/dev_overrides/docker-compose.interfacing.yaml

@@ -12,7 +12,7 @@ services:
       service: sensor_interfacing
     command: tail -F anything
     volumes:
-      - ${MONO_DIR}/src/sensor_interfacing:/home/bolty/ament_ws/src/sensor_interfacing
+      - ${MONO_DIR}/src/interfacing/sensor_interfacing:/home/bolty/ament_ws/src/sensor_interfacing
 
   can_interfacing:
     <<: *fixuid

modules/docker-compose.interfacing.yaml

@@ -10,7 +10,14 @@ services:
         - "${INTERFACING_SENSOR_IMAGE}:main"
       target: deploy
     image: "${INTERFACING_SENSOR_IMAGE}:${TAG}"
-    command: /bin/bash -c "ros2 launch sensor_interfacing sensor_interfacing.launch.py"
+    command: /bin/bash -c "ros2 launch sensor_interfacing all_sensors.launch.py"
+    network_mode: "host"
+
+    environment:
+      DISPLAY: $DISPLAY
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      - ~/.Xauthority:/root/.Xauthority
 
   can_interfacing:
     build:

src/interfacing/sensor_interfacing/CMakeLists.txt

@@ -7,8 +7,10 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-ament_package()
+ament_auto_package(INSTALL_TO_SHARE launch config)

src/interfacing/sensor_interfacing/config/VLP32C.yaml

@@ -0,0 +1,14 @@
+velodyne_driver_node:
+    ros__parameters:
+        device_ip: 192.168.1.202
+        gps_time: false
+        time_offset: 0.0
+        enabled: true
+        read_once: false
+        read_fast: false
+        repeat_delay: 0.0
+        frame_id: velodyne
+        model: 32C
+        rpm: 1200.0
+        port: 2368
+        timestamp_first_packet: false

src/interfacing/sensor_interfacing/config/blackfly_s.yaml

@@ -0,0 +1,157 @@
+#
+# config file for blackfly S cameras (USB3 and GigE)
+#
+# This file maps the ros parameters to the corresponding Spinnaker "nodes" in the camera.
+# For more details on how to modify this file, see the README on camera configuration files.
+
+parameters:
+  #
+  # -------- image format control
+  #
+  - name: pixel_format
+    type: enum
+    # Check available values with SpinView. Not all are supported by ROS!
+    # Some formats are e.g. "Mono8", "BayerRG8", "BGR8", "BayerRG16"
+    # default is "BayerRG8"
+    node: ImageFormatControl/PixelFormat
+  - name: binning_x
+    type: int
+    node: ImageFormatControl/BinningHorizontal
+  - name: binning_y
+    type: int
+    node: ImageFormatControl/BinningVertical
+  - name: image_width
+    type: int
+    node: ImageFormatControl/Width
+  - name: image_height
+    type: int
+    node: ImageFormatControl/Height
+  - name: offset_x # offset must come after image width reduction!
+    type: int
+    node: ImageFormatControl/OffsetX
+  - name: offset_y
+    type: int
+    node: ImageFormatControl/OffsetY
+
+  #
+  # -------- analog control
+  #
+  - name: gain_auto
+    type: enum
+    # valid values are "Continuous", "Off"
+    node: AnalogControl/GainAuto
+  - name: gain
+    type: float
+    node: AnalogControl/Gain
+  #
+  # -------- device link throughput limiting
+  #
+  - name: device_link_throughput_limit
+    type: int
+    node: DeviceControl/DeviceLinkThroughputLimit
+  #
+  # -------- transport layer control (GigE)
+  #
+  - name: gev_scps_packet_size
+    type: int
+    # default is 1400. Set to 9000 to enable jumbo frames, ensure NIC MTU set >= 9000
+    node: TransportLayerControl/GigEVision/GevSCPSPacketSize
+  #
+  # -------- digital IO control
+  #
+  - name: line0_selector # black wire: opto-isolated input
+    type: enum
+    node: DigitalIOControl/LineSelector
+  - name: line1_selector # white wire: opto-isolated output
+    type: enum
+    node: DigitalIOControl/LineSelector
+  - name: line1_linemode # valid values: "Input", "Output"
+    type: enum
+    node: DigitalIOControl/LineMode
+  - name: line2_selector # red wire: non-isolated input/output
+    type: enum
+    node: DigitalIOControl/LineSelector
+  - name: line2_v33enable # red wire: 3.3V power
+    type: bool
+    node: DigitalIOControl/V3_3Enable
+  - name: line3_selector # green wire: aux voltage input and non-isolated input
+    type: enum
+    node: DigitalIOControl/LineSelector
+  - name: line3_linemode # valid values: "Input", "Output"
+    type: enum
+    node: DigitalIOControl/LineMode
+  #
+  # -------- acquisition control
+  #
+  - name: exposure_auto
+    type: enum
+    # valid values are "Off", "Continuous"
+    node: AcquisitionControl/ExposureAuto
+  - name: exposure_time
+    type: float
+    node: AcquisitionControl/ExposureTime
+  - name: frame_rate_enable
+    type: bool
+    node: AcquisitionControl/AcquisitionFrameRateEnable
+  - name: frame_rate
+    type: float
+    node: AcquisitionControl/AcquisitionFrameRate
+  - name: trigger_selector
+    type: enum
+    # valid values are e.g. "FrameStart", "AcquisitionStart", "FrameBurstStart"
+    node: AcquisitionControl/TriggerSelector
+  - name: trigger_mode
+    type: enum
+    # valid values are "On" and "Off"
+    node: AcquisitionControl/TriggerMode
+  - name: trigger_source
+    type: enum
+    # valid values are "Line<0,1,2>", "UserOutput<0,1,2>", "Counter<0,1><Start/End>",
+    # "LogicBlock<0,1>
+    node: AcquisitionControl/TriggerSource
+  - name: trigger_delay
+    # value >= 9
+    type: float
+    node: AcquisitionControl/TriggerDelay
+  - name: trigger_overlap
+    type: enum
+    # valid values: "Off" and "ReadOut"
+    node: AcquisitionControl/TriggerOverlap
+  - name: trigger_activation
+    type: enum
+    # valid values: "LevelLow", "LevelHigh", "FallingEdge", "RisingEdge", "AnyEdge",
+    node: AcquisitionControl/TriggerActivation
+  #
+  # --------- chunk control
+  #
+  - name: chunk_mode_active
+    type: bool
+    node: ChunkDataControl/ChunkModeActive
+  - name: chunk_selector_frame_id
+    type: enum
+    # valid values: "FrameID"
+    node: ChunkDataControl/ChunkSelector
+  - name: chunk_enable_frame_id
+    type: bool
+    node: ChunkDataControl/ChunkEnable
+  - name: chunk_selector_exposure_time
+    type: enum
+    # valid values: "ExposureTime"
+    node: ChunkDataControl/ChunkSelector
+  - name: chunk_enable_exposure_time
+    type: bool
+    node: ChunkDataControl/ChunkEnable
+  - name: chunk_selector_gain
+    type: enum
+    # valid values: "Gain"
+    node: ChunkDataControl/ChunkSelector
+  - name: chunk_enable_gain
+    type: bool
+    node: ChunkDataControl/ChunkEnable
+  - name: chunk_selector_timestamp
+    type: enum
+    # valid values: "Timestamp"
+    node: ChunkDataControl/ChunkSelector
+  - name: chunk_enable_timestamp
+    type: bool
+    node: ChunkDataControl/ChunkEnable

src/interfacing/sensor_interfacing/launch/all_sensors.launch.py

@@ -0,0 +1,48 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.conditions import LaunchConfigurationEquals
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    launch_cameras = LaunchConfiguration('launch_cameras', default=True)
+    launch_cameras_arg = DeclareLaunchArgument('launch_cameras', default_value=launch_cameras,
+                                               description='Launch Flir Blackfly S Cameras')
+    launch_lidars = LaunchConfiguration('launch_lidars', default=True)
+    launch_lidars_arg = DeclareLaunchArgument('launch_lidars', default_value=launch_lidars,
+                                              description='Launch Velodyne LiDAR')
+    launch_args = [launch_cameras_arg,
+                   launch_lidars_arg]
+
+    sensor_interfacing_include_dir = os.path.join(
+        get_package_share_directory('sensor_interfacing'), 'launch', 'include')
+
+    urdf_static_transform_publisher_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [sensor_interfacing_include_dir, '/urdf.launch.py'])
+    )
+
+    cameras_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [sensor_interfacing_include_dir, '/cameras.launch.py']),
+        condition=LaunchConfigurationEquals(
+            'launch_cameras', 'True')
+    )
+
+    lidars_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [sensor_interfacing_include_dir, '/lidars.launch.py']),
+        condition=LaunchConfigurationEquals(
+            'launch_lidars', 'True')
+    )
+
+    return LaunchDescription(launch_args + [
+        # urdf_static_transform_publisher_launch, # uncomment once we have a URDF
+        cameras_launch,
+        lidars_launch
+    ])

src/interfacing/sensor_interfacing/launch/include/cameras.launch.py

@@ -0,0 +1,60 @@
+import os
+
+from ament_index_python.packages 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():
+    # Reference from https://github.com/ros-drivers/flir_camera_driver/blob/humble-devel/spinnaker_camera_driver/launch/driver_node.launch.py
+
+    parameter_file = LaunchConfiguration('parameter_file', default=os.path.join(
+        get_package_share_directory('spinnaker_camera_driver'), 'config', 'blackfly_s.yaml'))
+    parameter_file_arg = DeclareLaunchArgument('parameter_file', default_value=parameter_file,
+                                               description='path to ros parameter definition file (override camera type)')
+    serial = LaunchConfiguration('serial', default="'18542606'")
+    serial_arg = DeclareLaunchArgument('serial', default_value=serial,
+                                       description='FLIR serial number of camera (in quotes!!)')
+
+    # extra_params = { 'debug': True,
+                    #  'device_link_throughput_limit': 125000000,
+                    #  'gev_scps_packet_size': 1440,
+                    #  }
+
+    camera_node = Node(
+        package='spinnaker_camera_driver',
+        executable='camera_driver_node',
+        output='screen',
+        name='flir_camera',
+        arguments=['--ros-args', '--log-level', 'debug'],
+        parameters=[
+            # extra_params,
+            {
+                    'parameter_file': parameter_file,
+                    'serial_number': [serial],
+                    }]
+                    ,
+        remappings=[
+            ('~/control', '/exposure_control/control'),
+        ])
+
+    debayer_node = Node(
+        package='image_proc',
+        executable='debayer_node',
+        output='screen',
+        name='debayer_node',
+        remappings=[
+            ('/image_raw', '/flir_camera/image_raw'),
+    ]
+    )
+
+
+    return LaunchDescription([
+        parameter_file_arg,
+        serial_arg,
+        camera_node,
+        debayer_node
+    ])