Add drivers for lidar and camera

docker/interfacing/sensor_interfacing/sensor_interfacing.Dockerfile

@@ -9,6 +9,16 @@ WORKDIR ${AMENT_WS}/src
 COPY src/interfacing/sensor_interfacing sensor_interfacing
 COPY src/wato_msgs/sample_msgs sample_msgs
 
+# Camera ROS2 Driver
+RUN git clone https://github.com/ros-drivers/flir_camera_driver.git && \
+    cd flir_camera_driver && \
+    git checkout humble-release
+
+# LiDAR ROS2 Driver
+RUN git clone https://github.com/ros-drivers/velodyne.git && \
+    cd velodyne && \
+    git checkout humble-devel
+
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
     rosdep install --from-paths . --ignore-src -r -s \

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,7 @@ 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"
 
   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 params)

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,37 @@
+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="'20435008'")
+    serial_arg = DeclareLaunchArgument('serial', default_value=serial,
+                                       description='FLIR serial number of camera (in quotes!!)')
+
+    camera_node = Node(
+        package='spinnaker_camera_driver',
+        executable='camera_driver_node',
+        output='screen',
+        name='flir_camera',
+        parameters=[{'parameter_file': parameter_file,
+                     'serial_number': [serial]}],
+        remappings=[
+            ('~/control', '/exposure_control/control'),
+        ])
+
+    return LaunchDescription([
+        parameter_file_arg,
+        serial_arg,
+        camera_node
+    ])

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

@@ -0,0 +1,31 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import RegisterEventHandler, EmitEvent
+from launch.events import Shutdown
+from launch.event_handlers import OnProcessExit
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Referenced from https://github.com/ros-drivers/velodyne/blob/ros2/velodyne_driver/launch/velodyne_driver_node-VLP32C-launch.py
+    velodyne_config_directory = os.path.join(get_package_share_directory(
+        'velodyne_driver'), 'config')
+    params = os.path.join(
+        velodyne_config_directory, 'VLP32C-velodyne_driver_node-params.yaml')
+    velodyne_driver_node = Node(package='velodyne_driver',
+                                executable='velodyne_driver_node',
+                                output='both',
+                                parameters=[params])
+
+    return LaunchDescription([
+        velodyne_driver_node,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=velodyne_driver_node,
+                on_exit=[EmitEvent(
+                    event=Shutdown())],
+            )),
+    ])

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

@@ -0,0 +1,27 @@
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
+                                             description='Running Simulation?')
+
+    urdf_filepath = '/nominals.urdf'
+
+    with open(urdf_filepath, 'r') as infp:
+        robot_desc = infp.read()
+
+    return launch.LaunchDescription([
+        use_sim_time_arg,
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{'use_sim_time': use_sim_time,
+                         'robot_description': robot_desc}],
+        ),
+    ])

src/interfacing/sensor_interfacing/package.xml

@@ -4,7 +4,7 @@
   <name>sensor_interfacing</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="e23zhou@uwaterloo.ca">bolty</maintainer>
+  <maintainer email="s36gong@uwaterloo.ca">Steven Gong</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>