diff --git a/.github/workflows/ros-docker-image.yaml b/.github/workflows/ros-docker-image.yaml new file mode 100644 index 0000000..3b90a14 --- /dev/null +++ b/.github/workflows/ros-docker-image.yaml @@ -0,0 +1,63 @@ +--- +name: Build/Publish ROS Docker Image + +on: + workflow_dispatch: + inputs: + build_type: + description: Is it a "development" or a "stable" release? + required: true + default: development + type: choice + options: + - development + - stable + target_distro: + description: In case of "stable" release specify the ROS distro of the existing docker image (eg. + humble) + type: string + default: ardent + target_release: + description: In case of "stable" release specify the version of the existing docker image (eg. + 1.0.12) + type: string + default: 0.0.0 + target_date: + description: In case of "stable" release specify the date of the existing docker image in format + YYYYMMDD (eg. 20220124) + type: string + default: '20131206' + repository_dispatch: + types: [rebuild] + pull_request: + types: + - closed + - opened + +jobs: + build: + runs-on: ubuntu-22.04 + strategy: + fail-fast: false + matrix: + ros_distro: [humble, jazzy] + + steps: + + - name: Checkout + uses: actions/checkout@v2 + + - name: Build Docker Image + uses: husarion-ci/ros-docker-img-action@v0.4 + with: + dockerhub_username: ${{ secrets.DOCKERHUB_USERNAME }} + dockerhub_token: ${{ secrets.DOCKERHUB_TOKEN }} + repo_name: livox-lidar + main_branch_name: ros2 + build_type: ${{ inputs.build_type }} + ros_distro: ${{ matrix.ros_distro }} + platforms: linux/amd64, linux/arm64 + # variables important only for stable release + target_distro: ${{ inputs.target_distro }} + target_release: ${{ inputs.target_release }} + target_date: ${{ inputs.target_date }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..cf03b95 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,56 @@ +--- +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.5.0 + hooks: + - id: check-merge-conflict + - id: trailing-whitespace + - id: end-of-file-fixer + - id: check-yaml + - id: check-xml + - id: check-added-large-files + - id: check-ast + - id: check-json + - id: name-tests-test + files: ^.*\/test\/.*$ + args: [--pytest-test-first] + + - repo: https://github.com/codespell-project/codespell + rev: v2.2.6 + hooks: + - id: codespell + entry: codespell * + + - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt + rev: 0.2.3 + hooks: + - id: yamlfmt + files: ^(?!.*compose).*$ + args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100'] + + + - repo: https://github.com/psf/black + rev: 23.12.0 + hooks: + - id: black + args: [--line-length=99] + + - repo: https://github.com/PyCQA/flake8 + rev: 6.1.0 + hooks: + - id: flake8 + args: ['--ignore=E501,W503'] # ignore too long line and line break before binary operator, + # black checks it + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: v1.1.1 + hooks: + - id: doc8 + args: [--max-line-length=100, --ignore=D001] + exclude: ^.*\/CHANGELOG\.rst/.*$ + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v17.0.6 + hooks: + - id: clang-format diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..aef36ed --- /dev/null +++ b/Dockerfile @@ -0,0 +1,48 @@ +# Define build stage for creating ROS packages +ARG ROS_DISTRO=jazzy +ARG PREFIX= + +# =========================== package builder =============================== +FROM husarnet/ros:$ROS_DISTRO-ros-base AS pkg-builder + +WORKDIR /ros2_ws + +# Setup workspace +RUN git clone https://github.com/tu-darmstadt-ros-pkg/livox_ros_driver2 -b $ROS_DISTRO /ros2_ws/src/livox_ros_driver2 && \ + git clone https://github.com/tu-darmstadt-ros-pkg/Livox-SDK2 -b $ROS_DISTRO /ros2_ws/src/livox_sdk2 && \ + apt-get update -y && \ + rosdep update --rosdistro $ROS_DISTRO && \ + rosdep install --from-paths src --ignore-src -y + + +# Optional: Create healthcheck package +RUN cd src/ && \ + source /opt/ros/$ROS_DISTRO/setup.bash && \ + ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp sensor_msgs && \ + sed -i '/find_package(sensor_msgs REQUIRED)/a \ + add_executable(healthcheck_node src/healthcheck.cpp)\n \ + ament_target_dependencies(healthcheck_node rclcpp sensor_msgs)\n \ + install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \ + /ros2_ws/src/healthcheck_pkg/CMakeLists.txt +COPY ./husarion_utils/healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ + +# Build +RUN source /opt/ros/$ROS_DISTRO/setup.bash && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + echo $(cat /ros2_ws/src/livox_ros_driver2/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') > /version.txt && \ + rm -rf build log + +RUN apt update -y && \ + apt-get install -y ros-$ROS_DISTRO-nav2-common && \ + apt-get clean && \ + rm -rf src && \ + rm -rf /var/lib/apt/lists/* + +COPY ./husarion_utils /husarion_utils + +HEALTHCHECK --interval=2s --timeout=1s --start-period=20s --retries=1 \ + CMD ["/husarion_utils/healthcheck.sh"] + + +# Ensure LIDAR stops spinning on container shutdown +STOPSIGNAL SIGINT diff --git a/README.md b/README.md index b85ed59..eda32f7 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,52 @@ -# livox-docker -Docker images for Livox LIDARs +

+ Docker Images for Livox LIDARs +

+ +The repository includes a GitHub Actions workflow that automatically deploys built Docker images to the [husarion/livox-docker](https://hub.docker.com/r/husarion/livox) Docker Hub repositories. This process is based on the fork of [tu-darmstadt-ros-pkg/livox_ros_driver2](https://github.com/tu-darmstadt-ros-pkg/livox_ros_driver2) repository. + +[![ROS Docker Image](https://github.com/husarion/livox-docker/actions/workflows/ros-docker-image.yaml/badge.svg)](https://github.com/husarion/livox-docker/actions/workflows//ros-docker-image.yaml) + + +## Prepare Environment + +1. Plugin the Device + +Connect the device to the power supply and plug the ethernet cable to your PC or Husarion UGV router. + +2. Check connection + +Read the serial number of a Livox MID360 and remember last 2 numbers. These numbers are the part of its ip `192.168.1.1xx`. + +To check the connection use the `ping` command: +```bash +ping 192.168.1.1xx +``` + +## Demo + + +1. Clone the Repository + + ```bash + git clone https://github.com/husarion/livox-docker.git + cd livox-docker/demo + ``` + +2. Set the Appropriate IP + In [configuration](./demo/MID360_config.json) `lidar_config` > `ip` in the 28th line set this ip. + + Your SBC should have ip `192.168.1.50`. For Husarion UGV SBC you can add to the ethernet interface configuration additional IP. + In file `/etc/netplan/01-network-manager-all.yaml` add to `addresses` `- 192.168.1.50/24`. + +3. Activate the Device + + ```bash + docker compose up livox + ``` + +4. Launch Visualization + + ```bash + xhost local:root + docker compose up rviz + ``` diff --git a/demo/.env b/demo/.env new file mode 100644 index 0000000..50043c4 --- /dev/null +++ b/demo/.env @@ -0,0 +1,7 @@ +# ======================================= +# Namespace config +# ======================================= +# To use multiple devices on multiple robots you have to specify following field + +# Uncomment to add robot namespace +# ROBOT_NAMESPACE=robot diff --git a/demo/MID360_config.json b/demo/MID360_config.json new file mode 100644 index 0000000..e06912d --- /dev/null +++ b/demo/MID360_config.json @@ -0,0 +1,42 @@ +{ + "lidar_summary_info": { + "lidar_type": 8 + }, + "MID360": { + "lidar_net_info": { + "cmd_data_port": 56100, + "push_msg_port": 56200, + "point_data_port": 56300, + "imu_data_port": 56400, + "log_data_port": 56500 + }, + "host_net_info": { + "cmd_data_ip": "192.168.1.50", + "cmd_data_port": 56101, + "push_msg_ip": "192.168.1.50", + "push_msg_port": 56201, + "point_data_ip": "192.168.1.50", + "point_data_port": 56301, + "imu_data_ip": "192.168.1.50", + "imu_data_port": 56401, + "log_data_ip": "192.168.1.50", + "log_data_port": 56501 + } + }, + "lidar_configs": [ + { + "ip": "192.168.1.160", + "pcl_data_type": 1, + "pattern_mode": 0, + "frame_id": "", + "extrinsic_parameter": { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} diff --git a/demo/compose.yaml b/demo/compose.yaml new file mode 100644 index 0000000..e5c658b --- /dev/null +++ b/demo/compose.yaml @@ -0,0 +1,33 @@ +x-common-config: + &common-config + network_mode: host + ipc: host + env_file: .env + +services: + livox: + # image: husarion/livox-lidar:jazzy + build: + context: .. + dockerfile: Dockerfile + <<: *common-config + volumes: + - ./MID360_config.json:/ros2_ws/install/livox_ros_driver2/share/livox_ros_driver2/config/MID360_config.json + environment: + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + command: > + ros2 launch /husarion_utils/livox.launch.py + + rviz: + image: husarion/rviz2:humble + <<: *common-config + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix:rw + - ./default.rviz:/root/.rviz2/default.rviz + - ./rviz.launch.py:/ros2_ws/rviz.launch.py + environment: + - DISPLAY=${DISPLAY:?err} + - LIBGL_ALWAYS_SOFTWARE=1 + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + command: > + ros2 launch rviz.launch.py diff --git a/demo/default.rviz b/demo/default.rviz new file mode 100644 index 0000000..31abb26 --- /dev/null +++ b/demo/default.rviz @@ -0,0 +1,179 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21/Status1 + Splitter Ratio: 0.6549618244171143 + Tree Height: 775 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Interact1 + - /2D Goal Pose1/Topic1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 + - Class: slam_toolbox::SlamToolboxPlugin + Name: SlamToolboxPlugin +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: livox/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: laser + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + - Class: rviz_visual_tools/KeyTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 18.35651969909668 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.13568797707557678 + Y: -0.006133674643933773 + Z: -0.6133196949958801 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5947961807250977 + Target Frame: panther/base_link + Value: Orbit (rviz) + Yaw: 3.9514670372009277 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1009 + Hide Left Dock: false + Hide Right Dock: false + Navigation 2: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + SlamToolboxPlugin: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1850 + X: 70 + Y: 34 diff --git a/demo/rviz.launch.py b/demo/rviz.launch.py new file mode 100644 index 0000000..507aaab --- /dev/null +++ b/demo/rviz.launch.py @@ -0,0 +1,75 @@ +from launch import LaunchDescription +from launch_ros.actions import Node, PushRosNamespace +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import EnvironmentVariable, LaunchConfiguration + + +def replace_rviz_fixed_frame(input_file, output_file, fix_frame): + with open(input_file, "r") as file: + rviz_content = file.read() + + rviz_content = rviz_content.replace("Fixed Frame: laser", f"Fixed Frame: {fix_frame}") + + with open(output_file, "w") as file: + file.write(rviz_content) + + +def launch_setup(context, *args, **kwargs): + robot_namespace = LaunchConfiguration("robot_namespace").perform(context) + device_namespace = LaunchConfiguration("device_namespace").perform(context) + + default_rviz_conf = "/root/.rviz2/default.rviz" + rviz_conf = "/root/.rviz2/modified_default.rviz" + + ns = "/" + if device_namespace: + frame_id = device_namespace + "_link" + ns = f"/{device_namespace}" + else: + frame_id = "laser" + + if robot_namespace: + ns_ext = robot_namespace + "/" + ns = f"/{robot_namespace}{ns}" + else: + ns_ext = "" + frame_id = ns_ext + frame_id + + replace_rviz_fixed_frame(default_rviz_conf, rviz_conf, frame_id) + + remapping = [] + if robot_namespace: + remapping.append(("/clicked_point", f"/{robot_namespace}/clicked_point")) + remapping.append(("/goal_pose", f"/{robot_namespace}/goal_pose")) + remapping.append(("/initialpose", f"/{robot_namespace}/initialpose")) + + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + namespace=device_namespace, + remappings=remapping, + arguments=["-d", rviz_conf], + output="screen", + ) + + return [PushRosNamespace(robot_namespace), rviz] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "robot_namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Namespace which will appear in front of all topics (including /tf and /tf_static).", + ), + DeclareLaunchArgument( + "device_namespace", + default_value="", + description="Sensor namespace that will appear after all topics and TF frames, used for distinguishing multiple cameras on the same robot.", + ), + OpaqueFunction(function=launch_setup), + PushRosNamespace(LaunchConfiguration("robot_namespace")), + ] + ) diff --git a/husarion_utils/healthcheck.cpp b/husarion_utils/healthcheck.cpp new file mode 100644 index 0000000..76c3d8d --- /dev/null +++ b/husarion_utils/healthcheck.cpp @@ -0,0 +1,66 @@ +#include "fstream" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +using namespace std::chrono_literals; + +#define HEALTHCHECK_PERIOD 500ms +#define MSG_VALID_TIME 2s + +using MsgType = sensor_msgs::msg::PointCloud2; + +class HealthCheckNode : public rclcpp::Node { +public: + HealthCheckNode() + : Node("healthcheck_livox"), + last_msg_time(std::chrono::steady_clock::now()) { + + // Set custom QoS settings to match the publisher + auto qos = rclcpp::QoS( + rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10)); + qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + subscription_ = create_subscription( + "livox/lidar", qos, + std::bind(&HealthCheckNode::msgCallback, this, std::placeholders::_1)); + + timer_ = create_wall_timer(HEALTHCHECK_PERIOD, + std::bind(&HealthCheckNode::healthyCheck, this)); + } + +private: + std::chrono::steady_clock::time_point last_msg_time; + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::TimerBase::SharedPtr timer_; + + void writeHealthStatus(const std::string &status) { + std::ofstream healthFile("/var/tmp/health_status.txt"); + healthFile << status; + } + + void msgCallback(const MsgType::SharedPtr msg) { + RCLCPP_DEBUG(get_logger(), "Msg arrived"); + last_msg_time = std::chrono::steady_clock::now(); + } + + void healthyCheck() { + auto current_time = std::chrono::steady_clock::now(); + auto elapsed_time = current_time - last_msg_time; + bool is_msg_valid = elapsed_time < MSG_VALID_TIME; + + if (is_msg_valid) { + writeHealthStatus("healthy"); + } else { + writeHealthStatus("unhealthy"); + } + } +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/husarion_utils/healthcheck.sh b/husarion_utils/healthcheck.sh new file mode 100755 index 0000000..7cae7a1 --- /dev/null +++ b/husarion_utils/healthcheck.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +HEALTHCHECK_FILE="/var/tmp/health_status.txt" + + +# Now check the health status +if [ -f "$HEALTHCHECK_FILE" ]; then + status=$(cat "$HEALTHCHECK_FILE") + if [ "$status" == "healthy" ]; then + exit 0 + else + exit 1 + fi +else + echo "Healthcheck file still not found. There may be an issue with the node." + exit 1 +fi diff --git a/husarion_utils/livox.launch.py b/husarion_utils/livox.launch.py new file mode 100755 index 0000000..0cea1ae --- /dev/null +++ b/husarion_utils/livox.launch.py @@ -0,0 +1,126 @@ +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + OpaqueFunction, +) +from launch.conditions import IfCondition +from launch_ros.actions import Node, PushRosNamespace +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString + + +def create_health_status_file(): + with open("/var/tmp/health_status.txt", "w") as file: + file.write("healthy") + + +def launch_setup(context, *args, **kwargs): + robot_namespace = LaunchConfiguration("robot_namespace").perform(context) + device_namespace = LaunchConfiguration("device_namespace").perform(context) + + ns = "/" + if device_namespace: + frame_id = device_namespace + "_link" + ns = f"/{device_namespace}" + else: + frame_id = "laser" + + if robot_namespace: + ns_ext = robot_namespace + "/" + ns = f"/{robot_namespace}{ns}" + else: + ns_ext = "" + frame_id = ns_ext + frame_id + + # Device Namespace + livox_actions = [] + livox_actions.append(PushRosNamespace(device_namespace)) + + livox_ns = GroupAction(actions=livox_actions) + + # Retrieve the healthcheck argument + healthcheck = LaunchConfiguration("healthcheck").perform(context) + + # Conditional file creation based on the healthcheck argument + if healthcheck == "False": + create_health_status_file() + + # Define the healthcheck node + healthcheck_node = Node( + package="healthcheck_pkg", + executable="healthcheck_node", + name="healthcheck_livox", + namespace=ns, + output="screen", + condition=IfCondition(healthcheck), + ) + + xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format + multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic + data_src = 0 # 0-lidar, others-Invalid data src + publish_freq = 10.0 # frequency of publish, 5.0, 10.0, 20.0, 50.0, etc. + output_type = 0 + lvx_file_path = "/home/livox/livox_test.lvx" + cmdline_bd_code = "livox0000000001" + user_config_path = PathJoinSubstitution( + [FindPackageShare("livox_ros_driver2"), "config", "MID360_config.json"] + ) + + user_config_path = ReplaceString( + source_file=user_config_path, + replacements={ + "": frame_id, + }, + ) + + livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code}, + ] + + livox_driver = Node( + package="livox_ros_driver2", + executable="livox_ros_driver2_node", + name="livox_lidar_publisher", + output="screen", + parameters=livox_ros2_params, + namespace=ns, + ) + + return [PushRosNamespace(robot_namespace), livox_ns, healthcheck_node, livox_driver] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "robot_namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Namespace which will appear in front of all topics.", + ), + DeclareLaunchArgument( + "device_namespace", + default_value="", + description="Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.", + ), + DeclareLaunchArgument( + "healthcheck", + default_value="False", + description="Enable health check for livox.", + ), + OpaqueFunction(function=launch_setup), + ] + )