feat(drive): add "lidar_to_laserscan" node

Author: onkoe

src/drive_launcher/launch/ebox.launch.py

@@ -2,11 +2,12 @@
 This file runs all required nodes to speak with the Ebox.
 """
 
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import OpaqueFunction
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
 from launch_ros.actions.node import Node
-from rclpy.impl.rcutils_logger import RcutilsLogger
-from rclpy.node import get_logger
 
 
 def generate_launch_description() -> LaunchDescription:
@@ -38,13 +39,38 @@ def generate_launch_description() -> LaunchDescription:
         executable="sensors_node",
     )
 
-    # TODO: implement the camera launch files, then replace this!
-    camera_warning: OpaqueFunction = OpaqueFunction(function=_warn_about_camera_todos)
-
-    return LaunchDescription([wheels_node, lights_node, sensors_node, camera_warning])
+    # Unitree L2 LiDAR bridge package.
+    pkg_soro_lidar: str = get_package_share_directory("soro_lidar")
+    lidar_launch: IncludeLaunchDescription = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [
+                        pkg_soro_lidar,
+                        "launch",
+                        "launch.py",
+                    ]
+                )
+            ]
+        )
+    )
 
+    # See3CAM color stream for ArUco marker detection.
+    pkg_see3cam: str = get_package_share_directory("see3cam")
+    see3cam_launch: IncludeLaunchDescription = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [
+                        pkg_see3cam,
+                        "launch",
+                        "see3cam.launch.py",
+                    ]
+                )
+            ]
+        )
+    )
 
-def _warn_about_camera_todos(_):
-    # we'll emit a diagnostic to remind ourselves lol
-    logger: RcutilsLogger = get_logger("ebox.launch.py")
-    _ = logger.error("TODO: implement cameras in launch file!")
+    return LaunchDescription(
+        [wheels_node, lights_node, sensors_node, lidar_launch, see3cam_launch]
+    )

src/drive_launcher/launch/helpers/lidar_to_laserscan.launch.py

@@ -0,0 +1,53 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description() -> LaunchDescription:
+    use_sim_time: LaunchConfiguration = LaunchConfiguration(
+        "use_sim_time", default="false"
+    )
+
+    pointcloud_to_laserscan: ComposableNode = ComposableNode(
+        name="lidar_pointcloud_to_laserscan",
+        package="pointcloud_to_laserscan",
+        plugin="pointcloud_to_laserscan::PointCloudToLaserScanNode",
+        remappings=[
+            ("cloud_in", "/unilidar/cloud"),
+            ("scan", "/scan"),
+        ],
+        parameters=[
+            {
+                "use_sim_time": use_sim_time,
+                "target_frame": "base_link",
+                "transform_tolerance": 0.2,
+                "min_height": -0.5,
+                "max_height": 2.5,
+                "angle_min": -3.1415926535,
+                "angle_max": 3.1415926535,
+                "angle_increment": 0.0058,
+                "scan_time": 0.1,
+                "range_min": 0.3,
+                "range_max": 100.0,
+                "use_inf": True,
+            }
+        ],
+    )
+
+    container: ComposableNodeContainer = ComposableNodeContainer(
+        name="lidar_scan_container",
+        namespace="",
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=[pointcloud_to_laserscan],
+        output="screen",
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("use_sim_time", default_value="false"),
+            container,
+        ]
+    )

src/drive_launcher/launch/rover.launch.py

@@ -53,6 +53,9 @@ def generate_launch_description() -> LaunchDescription:
     # directly by our Navigator).
     robot_localization: IncludeLaunchDescription = _robot_localization(use_sim_time)
 
+    # Convert Unitree LiDAR point clouds into LaserScan for scan-based nodes
+    # (e.g., slam_toolbox and collision monitor).
+    lidar_to_laserscan: IncludeLaunchDescription = _lidar_to_laserscan(use_sim_time)
 
     # ok, so this part launches a few different things...
     #
@@ -111,6 +114,7 @@ def generate_launch_description() -> LaunchDescription:
             utm_conversion_node,
             robot_state_publisher,
             robot_localization,
+            lidar_to_laserscan,
             navigation_stack,
             ros2_control,
             Node(
@@ -187,7 +191,7 @@ def _navigation_bringup(
     )
 
 
-def _depthimage_to_laserscan(
+def _lidar_to_laserscan(
     use_sim_time: LaunchConfiguration,
 ) -> IncludeLaunchDescription:
     pkg_drive_launcher: str = get_package_share_directory("drive_launcher")
@@ -199,7 +203,7 @@ def _depthimage_to_laserscan(
                         pkg_drive_launcher,
                         "launch",
                         "helpers",
-                        "depthimage_to_laserscan.launch.py",
+                        "lidar_to_laserscan.launch.py",
                     ]
                 )
             ]