Provide waffle_pi Camera Functionality Under Humble

turtlebot3_bringup/CMakeLists.txt

@@ -21,7 +21,7 @@ find_package(ament_cmake REQUIRED)
 # Install
 ################################################################################
 install(
-  DIRECTORY launch param script
+  DIRECTORY launch param script camera_info
   DESTINATION share/${PROJECT_NAME}
 )
 

turtlebot3_bringup/camera_info/turtlebot3_rpicamera.yaml

@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [495.5130372726824, 0.0, 309.64896771170527, 0.0, 495.2808626986745, 235.54576124624984, 0.0, 0.0, 1.0]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0.1956156852552275, -0.402686946976495, -0.0017960602101275867, 0.0002452730384824211, 0.14615875806533088]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [495.5130372726824, 0.0, 309.64896771170527, 0.0, 0.0, 495.2808626986745, 235.54576124624984, 0.0, 0.0, 0.0, 1.0, 0.0]

turtlebot3_bringup/launch/image_transport.launch.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    return LaunchDescription([
+
+        Node(
+            package='image_transport',
+            executable='republish',
+            arguments=['compressed', 'raw'],
+            remappings=[
+                ('in/compressed', '/camera/image_raw/compressed'),
+                ('out', '/camera/image_raw'),
+            ],
+            output='screen'
+            ),
+    ])

turtlebot3_bringup/launch/robot.launch.py

@@ -26,6 +26,7 @@
 from launch.substitutions import LaunchConfiguration
 from launch.substitutions import ThisLaunchFileDir
 from launch_ros.actions import Node
+from launch.conditions import IfCondition
 
 
 def generate_launch_description():
@@ -58,6 +59,11 @@ def generate_launch_description():
 
     use_sim_time = LaunchConfiguration('use_sim_time', default='false')
 
+    launch_camera = LaunchConfiguration('launch_camera')
+    camera_default = 'false'
+    if TURTLEBOT3_MODEL == 'waffle_pi':
+        camera_default = 'true'
+
     return LaunchDescription([
         DeclareLaunchArgument(
             'use_sim_time',
@@ -74,12 +80,21 @@ def generate_launch_description():
             default_value=tb3_param_dir,
             description='Full path to turtlebot3 parameter file to load'),
 
+        DeclareLaunchArgument('launch_camera', default_value=camera_default,
+                              description='Determines if raspberry pi camera is launched.'),
+
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 [ThisLaunchFileDir(), '/turtlebot3_state_publisher.launch.py']),
             launch_arguments={'use_sim_time': use_sim_time}.items(),
         ),
 
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [ThisLaunchFileDir(), '/rpicamera.launch.py']),
+                condition=IfCondition(launch_camera)
+        ),
+
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource([lidar_pkg_dir, LDS_LAUNCH_FILE]),
             launch_arguments={'port': '/dev/ttyUSB0', 'frame_id': 'base_scan'}.items(),

turtlebot3_bringup/launch/rpicamera.launch.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    camera_params = LaunchConfiguration(
+        'camera_params',
+        default=os.path.join(
+            get_package_share_directory('turtlebot3_bringup'),
+            'param',
+            'camera.yaml'))
+
+    return LaunchDescription([
+
+        Node(
+            package='usb_cam',
+            executable='usb_cam_node_exe',
+            parameters=[camera_params],
+            arguments=['--ros-args', '--remap', '__ns:=/camera'],
+            remappings=[
+                ('/camera/image_raw', '/camera/image_raw/uncompressed'),
+            ],
+            output='screen'
+            ),
+    ])

turtlebot3_bringup/launch/rviz2.launch.py

@@ -24,10 +24,17 @@
 
 
 def generate_launch_description():
+    TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+
+    if TURTLEBOT3_MODEL == 'waffle_pi':
+        rviz_file = 'waffle_pi.rviz'
+    else:
+        rviz_file = 'model.rviz'
+
     rviz_config_dir = os.path.join(
         get_package_share_directory('turtlebot3_description'),
         'rviz',
-        'model.rviz')
+        rviz_file)
 
     return LaunchDescription([
         Node(

turtlebot3_bringup/package.xml

@@ -19,6 +19,9 @@
   <exec_depend>rviz2</exec_depend>
   <exec_depend>turtlebot3_description</exec_depend>
   <exec_depend>turtlebot3_node</exec_depend>
+  <exec_depend>usb_cam</exec_depend>
+  <exec_depend>image_tools</exec_depend>
+  <exec_depend>compressed_image_transport</exec_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

turtlebot3_bringup/param/camera.yaml

@@ -0,0 +1,9 @@
+/**:
+    ros__parameters:
+      framerate: 10.0
+      frame_id: "camera_rgb_optical_frame"
+      pixel_format: "yuyv2rgb"
+      image_width: 640
+      image_height: 480
+      camera_name: "camera"
+      camera_info_url: "package://turtlebot3_bringup/camera_info/turtlebot3_rpicamera.yaml"