Support LiDAR

pixi.toml

@@ -162,9 +162,15 @@ ros-humble-rviz2 = { channel = "robostack-humble" }
 ros-humble-rviz-default-plugins = { channel = "robostack-humble" }
 ros-humble-xacro = { channel = "robostack-humble" }
 
-# note: we can only depend on `video4linux`... on Linux. lol
+# required by LiDAR
+ros-humble-pcl-conversions = { channel = "robostack-humble" }
+pcl = { channel = "conda-forge" }
+
 [target.linux.dependencies]
+# note: we can only depend on `video4linux`... on Linux. lol
 ros-humble-v4l2-camera = { channel = "robostack-humble" }
+# and yea this only runs on linux for one reason or another
+ros-humble-spatio-temporal-voxel-layer = { channel = "robostack-humble" }
 
 [pypi-dependencies]
 colcon-ros-cargo = ">=0.2.0"

src/drive_launcher/launch/drive.launch.py

@@ -5,17 +5,19 @@
 """
 
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.substitutions.find_package import get_package_share_directory
 
 
 def generate_launch_description() -> LaunchDescription:
     pkg_drive_launcher: str = get_package_share_directory("drive_launcher")
+    use_sim_time = LaunchConfiguration("use_sim_time")
 
     return LaunchDescription(
         [
+            DeclareLaunchArgument("use_sim_time", default_value="false"),
             # `rover.launch.py` is the ros 2 side of things.
             #
             # it includes stuff like Nav2 and `slam_toolbox` for interpreting
@@ -31,7 +33,8 @@ def generate_launch_description() -> LaunchDescription:
                             ]
                         )
                     ]
-                )
+                ),
+                launch_arguments=[("use_sim_time", use_sim_time)],
             ),
             #
             # and `ebox.launch.py` is the ebox <=> autonomous communication

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,37 @@ 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.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,66 @@
+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,
+                # keep scan in LiDAR frame to avoid unnecessary per-message TF
+                # transforms and message-filter backpressure.
+                "target_frame": "unilidar_lidar",
+                "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,
+                "queue_size": 50,
+                "qos_overrides": {
+                    "/scan": {
+                        "publisher": {
+                            "reliability": "reliable",
+                            "history": "keep_last",
+                            "depth": 10,
+                            "durability": "volatile",
+                        }
+                    }
+                },
+            }
+        ],
+    )
+
+    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/helpers/navigation_stack.launch.py

@@ -4,6 +4,7 @@
     DeclareLaunchArgument,
     IncludeLaunchDescription,
     RegisterEventHandler,
+    TimerAction,
 )
 from launch.event_handlers import OnProcessExit, OnProcessStart
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -89,6 +90,10 @@ def generate_launch_description() -> LaunchDescription:
     # the `slam_toolbox::async_slam_toolbox_node` fills the `/tf:map` and
     # `tf:odom` frames with our translated laserscan data.
     slam_toolbox: ExecuteProcess = _slam_toolbox(use_sim_time)
+    delayed_slam_toolbox: TimerAction = TimerAction(
+        period=2.5,
+        actions=[slam_toolbox],
+    )
 
     navigation_bringup_node: Node = Node(
         package="navigator",
@@ -108,7 +113,7 @@ def generate_launch_description() -> LaunchDescription:
             SetParameter("use_sim_time", use_sim_time),
             #
             # first, the toolbox
-            slam_toolbox,
+            delayed_slam_toolbox,
             #
             # after that, we can launch the lil watcher node
             RegisterEventHandler(
@@ -167,7 +172,11 @@ def _slam_toolbox(
             "drive_launcher",
             "slam_toolbox.launch.py",
             # 😮‍💨 yet another hack...
-            PythonExpression(["'use_sim_time:=' + str(", use_sim_time, ")"]),
+            # quote launch config value so PythonExpression doesn't evaluate
+            # `false`/`true` as Python names.
+            PythonExpression(
+                ["'use_sim_time:=' + str('", use_sim_time, "')"]
+            ),
         ],
         shell=True,
         output="screen",

src/drive_launcher/launch/helpers/robot_localization.launch.py

@@ -46,7 +46,7 @@ def generate_launch_description() -> LaunchDescription:
         # wihtout this (not great ergonomics lol)
         remappings=[
             ("/gps/fix", "/sensors/gps"),  # sensor_msgs::msg::NavSatFix
-            ("/imu/data", "/sensors/imu"),  # sensor_msgs::msg::Imu
+            ("/imu/data", "/unilidar/imu"),  # sensor_msgs::msg::Imu
             # (keep... /odometry/filtered), # sensor_msgs::msg::Odometry
         ],
     )
@@ -82,7 +82,7 @@ def generate_launch_description() -> LaunchDescription:
         respawn=True,
         remappings=[
             ("/gps/fix", "/sensors/gps"),  # sensor_msgs::msg::NavSatFix
-            ("/imu/data", "/sensors/imu"),  # sensor_msgs::msg::Imu
+            ("/imu/data", "/unilidar/imu"),  # sensor_msgs::msg::Imu
             # (keep... /odometry/filtered),
         ],
     )
@@ -118,7 +118,7 @@ def generate_launch_description() -> LaunchDescription:
         respawn=True,
         remappings=[
             ("/gps/fix", "/sensors/gps"),  # sensor_msgs::msg::NavSatFix
-            ("/imu/data", "/sensors/imu"),  # sensor_msgs::msg::Imu
+            ("/imu/data", "/unilidar/imu"),  # sensor_msgs::msg::Imu
             ("/odometry/filtered", "/ekf_filter_node_map/odometry/filtered"),
         ],
     )

src/drive_launcher/launch/helpers/robot_state_publisher.launch.py

@@ -16,9 +16,14 @@ def generate_launch_description() -> LaunchDescription:
     starts the `robot_state_publisher::robot_state_publisher` node.
     """
     use_sim_time = LaunchConfiguration("use_sim_time")
+    ros2_control_plugin = LaunchConfiguration("ros2_control_plugin")
+    use_gazebo_ros2_control = LaunchConfiguration("use_gazebo_ros2_control")
 
     # grab the rover's udrf
-    robot_desc: dict[str, ParameterValue] = _grab_robot_description()
+    robot_desc: dict[str, ParameterValue] = _grab_robot_description(
+        ros2_control_plugin=ros2_control_plugin,
+        use_gazebo_ros2_control=use_gazebo_ros2_control,
+    )
 
     # the `robot_state_publisher` node basically takes the various transform
     # differences defined in a URDF file and tells other nodes where stuff on
@@ -40,12 +45,23 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument(
                 "use_sim_time",
             ),
+            DeclareLaunchArgument(
+                "ros2_control_plugin",
+                default_value="fake_components/GenericSystem",
+            ),
+            DeclareLaunchArgument(
+                "use_gazebo_ros2_control",
+                default_value="false",
+            ),
             rover_state_publisher_node,
         ]
     )
 
 
-def _grab_robot_description() -> dict[str, ParameterValue]:
+def _grab_robot_description(
+    ros2_control_plugin: LaunchConfiguration,
+    use_gazebo_ros2_control: LaunchConfiguration,
+) -> dict[str, ParameterValue]:
     """grabs the robot desc."""
 
     # make the description
@@ -60,6 +76,12 @@ def _grab_robot_description() -> dict[str, ParameterValue]:
                     "rover.urdf.xacro.xml",
                 ]
             ),
+            " ",
+            "ros2_control_plugin:=",
+            ros2_control_plugin,
+            " ",
+            "use_gazebo_ros2_control:=",
+            use_gazebo_ros2_control,
         ]
     )