From bd3ffd2715224b3f7b7066f914b5ec484060a550 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Fri, 20 Mar 2026 20:45:43 +0000 Subject: [PATCH 1/7] Initial plan From 27baa3b8d5bef5855899620bb25c1fc0ce3e2c37 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Fri, 20 Mar 2026 21:03:02 +0000 Subject: [PATCH 2/7] feat: single-node dual-lidar launch + all logging/thread-safety fixes from sub-pr-1 Co-authored-by: marc-hanheide <1153084+marc-hanheide@users.noreply.github.com> Agent-Logs-Url: https://github.com/jabasai/livox_ros_driver2/sessions/43d3b612-f959-4035-a1e5-f734d1e71bf7 --- .devcontainer/Dockerfile | 54 +++ .devcontainer/devcontainer.json | 38 ++ .devcontainer/docker-compose.yml | 23 ++ .github/copilot-instructions.md | 44 +++ .github/workflows/copilot-setup-steps.yml | 60 +++ .github/workflows/ros-ci.yml | 80 ++++ .gitignore | 2 +- CMakeLists.txt | 47 ++- README.md | 315 ++++++++++++---- config/MID360_config_167.json | 41 +++ config/MID360_config_184.json | 41 +++ launch/dual_mid360_launch.py | 342 +++++++++++++++++ launch/mid360_launch.py | 343 ++++++++++++++++++ {launch_ROS2 => launch}/msg_HAP_launch.py | 0 {launch_ROS2 => launch}/msg_MID360_launch.py | 0 {launch_ROS2 => launch}/rviz_HAP_launch.py | 0 {launch_ROS2 => launch}/rviz_MID360_launch.py | 0 {launch_ROS2 => launch}/rviz_mixed.py | 0 launch_ROS1/msg_HAP.launch | 40 -- launch_ROS1/msg_MID360.launch | 40 -- launch_ROS1/msg_mixed.launch | 40 -- launch_ROS1/rviz_HAP.launch | 45 --- launch_ROS1/rviz_MID360.launch | 45 --- launch_ROS1/rviz_mixed.launch | 45 --- package.xml | 35 ++ src/call_back/lidar_common_callback.cpp | 16 +- src/call_back/livox_lidar_callback.cpp | 294 ++++++++++----- src/comm/comm.h | 11 +- src/comm/pub_handler.cpp | 65 +++- src/comm/pub_handler.h | 7 + src/driver_node.h | 4 + src/include/livox_log.h | 52 +++ src/lddc.cpp | 66 ++-- src/lds.cpp | 22 +- src/lds_lidar.cpp | 24 +- src/livox_ros_driver2.cpp | 96 +++++ src/tools/livox_scan.cpp | 300 +++++++++++++++ src/tools/livox_set_ip.cpp | 277 ++++++++++++++ 38 files changed, 2488 insertions(+), 466 deletions(-) create mode 100644 .devcontainer/Dockerfile create mode 100644 .devcontainer/devcontainer.json create mode 100644 .devcontainer/docker-compose.yml create mode 100644 .github/copilot-instructions.md create mode 100644 .github/workflows/copilot-setup-steps.yml create mode 100644 .github/workflows/ros-ci.yml create mode 100644 config/MID360_config_167.json create mode 100644 config/MID360_config_184.json create mode 100644 launch/dual_mid360_launch.py create mode 100644 launch/mid360_launch.py rename {launch_ROS2 => launch}/msg_HAP_launch.py (100%) rename {launch_ROS2 => launch}/msg_MID360_launch.py (100%) rename {launch_ROS2 => launch}/rviz_HAP_launch.py (100%) rename {launch_ROS2 => launch}/rviz_MID360_launch.py (100%) rename {launch_ROS2 => launch}/rviz_mixed.py (100%) delete mode 100644 launch_ROS1/msg_HAP.launch delete mode 100644 launch_ROS1/msg_MID360.launch delete mode 100644 launch_ROS1/msg_mixed.launch delete mode 100644 launch_ROS1/rviz_HAP.launch delete mode 100644 launch_ROS1/rviz_MID360.launch delete mode 100644 launch_ROS1/rviz_mixed.launch create mode 100644 package.xml create mode 100644 src/include/livox_log.h create mode 100644 src/tools/livox_scan.cpp create mode 100644 src/tools/livox_set_ip.cpp diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 00000000..78da363e --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,54 @@ +FROM ros:humble + +ARG DEBIAN_FRONTEND=noninteractive + +# Install system and ROS2 dependencies +RUN apt-get update && apt-get install -y \ + build-essential \ + cmake \ + git \ + wget \ + curl \ + python3-colcon-common-extensions \ + python3-rosdep \ + python3-vcstool \ + libpcl-dev \ + libapr1-dev \ + libaprutil1-dev \ + ros-humble-rclcpp \ + ros-humble-rclcpp-components \ + ros-humble-std-msgs \ + ros-humble-sensor-msgs \ + ros-humble-rcl-interfaces \ + ros-humble-pcl-conversions \ + ros-humble-pcl-ros \ + ros-humble-rosbag2 \ + ros-humble-rosidl-default-generators \ + ros-humble-rosidl-default-runtime \ + ros-humble-ament-lint-auto \ + ros-humble-ament-lint-common \ + && rm -rf /var/lib/apt/lists/* + +RUN \ + echo "Installing livox_sdk2..." && \ + REPO_DIR="/tmp/livox_sdk2_repo" && \ + mkdir -p "$REPO_DIR" && \ + git clone --branch v1.2.5 --depth 1 https://github.com/Livox-SDK/Livox-SDK2.git "$REPO_DIR/Livox-SDK2" && \ + cd "$REPO_DIR/Livox-SDK2" && \ + mkdir -p build && \ + cd build && \ + rm -rf * && \ + cmake .. && \ + make -j$(nproc) && \ + make install && \ + echo "livox_sdk2 installed successfully." && \ + cd "/" && \ + rm -rf "$REPO_DIR" && \ + ldconfig + + +# Source ROS2 in every new shell +RUN echo "source /opt/ros/humble/setup.bash" >> /etc/bash.bashrc + +# Set up workspace directory +WORKDIR /ros2_ws diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 00000000..8a1cc4ec --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,38 @@ +{ + "name": "livox_ros_driver2 (ROS2 Humble)", + "dockerComposeFile": "docker-compose.yml", + "service": "devcontainer", + "workspaceFolder": "/ros2_ws/src/livox_ros_driver2", + "remoteEnv": { + "ROS_DISTRO": "humble", + "AMENT_PREFIX_PATH": "/opt/ros/humble", + "ROS_DOMAIN_ID": "0" + }, + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools", + "ms-vscode.cmake-tools", + "ms-python.python" + ], + "settings": { + "terminal.integrated.defaultProfile.linux": "bash", + "terminal.integrated.profiles.linux": { + "bash": { + "path": "/bin/bash", + "args": ["--login"] + } + }, + "cmake.buildDirectory": "/ros2_ws/build/livox_ros_driver2", + "C_Cpp.default.includePath": [ + "/opt/ros/humble/include/**", + "/ros2_ws/src/livox_ros_driver2/src/**", + "/ros2_ws/src/livox_ros_driver2/3rdparty/**" + ] + } + } + }, + "postCreateCommand": "cd /ros2_ws && bash -c 'source /opt/ros/humble/setup.bash && cp src/livox_ros_driver2/package_ROS2.xml src/livox_ros_driver2/package.xml'", + "postStartCommand": ". /opt/ros/humble/setup.sh", + "features": {} +} diff --git a/.devcontainer/docker-compose.yml b/.devcontainer/docker-compose.yml new file mode 100644 index 00000000..542e46df --- /dev/null +++ b/.devcontainer/docker-compose.yml @@ -0,0 +1,23 @@ +version: '3.8' + +services: + devcontainer: + build: + context: .. + dockerfile: .devcontainer/Dockerfile + volumes: + - ..:/ros2_ws/src/livox_ros_driver2:cached + networks: + default: {} + livox_net: + ipv4_address: 192.168.1.10 + command: sleep infinity + +networks: + livox_net: + driver: bridge + ipam: + driver: default + config: + - subnet: 192.168.1.0/24 + gateway: 192.168.1.1 diff --git a/.github/copilot-instructions.md b/.github/copilot-instructions.md new file mode 100644 index 00000000..c7af4610 --- /dev/null +++ b/.github/copilot-instructions.md @@ -0,0 +1,44 @@ +# Livox ROS Driver 2 — Copilot Instructions + +## Project Overview + +This is a ROS2 device driver for Livox 3D LiDAR sensors (MID360, HAP). It targets **ROS2 Humble or later**. The codebase also retains partial ROS1 compatibility gated behind `#ifdef BUILDING_ROS1` preprocessor guards. But only ever work on the ROS2 code. + +The driver supports running a **single node that manages multiple lidars** (the recommended approach for dual MID360 setups): one SDK instance handles both lidars, eliminating cross-instance SDK race conditions. See `launch/dual_mid360_launch.py`. + +## Build and Test + +```bash +# From the workspace root (/ros2_ws) +colcon build + +``` + +## Architecture + +- **`src/driver_node.*`** — ROS2 node entry point, lifecycle management; includes watchdog timer and Race-3 reconnect timer +- **`src/lddc.*`** — Lidar Data Distribution Center; routes point cloud and IMU data to ROS publishers +- **`src/lds_lidar.*` / `src/lds.*`** — Lidar Device Scheduler; manages sensor connections and data pipelines +- **`src/comm/`** — Low-level communication utilities: queues, semaphores, caching, and the central `PubHandler` (with per-handle allowlist) +- **`src/call_back/`** — SDK callback handlers for incoming lidar/IMU frames +- **`src/parse_cfg_file/`** — JSON config file parsing (uses RapidJSON from `3rdparty/`) +- **`src/include/`** — Shared headers; `ros_headers.h` abstracts ROS1/ROS2 includes; `livox_log.h` provides node-free logging macros +- **`config/`** — Per-sensor JSON configuration files +- **`launch/`** — ROS2 Python launch files; `dual_mid360_launch.py` launches ONE node for BOTH MID360 lidars +- **`src/tools/`** — Standalone CLI tools: `livox_scan` (discover lidars) and `livox_set_ip` (configure lidar IPs) + +## Code Conventions + +- **Language**: C++14, namespace `livox_ros` +- **Naming**: `PascalCase` for classes, `snake_case` for functions and member variables +- **Header guards**: `#ifndef LIVOX__H` / `#define LIVOX__H` +- **ROS abstraction**: Use the wrappers in `src/include/ros_headers.h` rather than including ROS headers directly; guard any ROS1-specific code with `#ifdef BUILDING_ROS1` +- **Logging**: Use `LIVOX_INFO` / `LIVOX_WARN` / `LIVOX_ERROR` / `LIVOX_DEBUG` macros from `src/include/livox_log.h` for node-free logging; they route to `RCLCPP_*` in ROS2 and `ROS_*` in ROS1 +- **Thread safety**: `connect_state` in `LidarDevice` is `std::atomic`; use `memory_order_acquire` / `memory_order_release` at all access sites +- **JSON parsing**: Use the bundled RapidJSON library in `3rdparty/rapidjson/` +- **License header**: All new source files must include the MIT license block present in existing files + +## Dependencies + +Key runtime deps: `rclcpp`, `rclcpp_components`, `sensor_msgs`, `pcl_conversions`, `livox_lidar_sdk`. +Do **not** add new third-party libraries without updating both `CMakeLists.txt` and `package.xml`. diff --git a/.github/workflows/copilot-setup-steps.yml b/.github/workflows/copilot-setup-steps.yml new file mode 100644 index 00000000..1ea59b9f --- /dev/null +++ b/.github/workflows/copilot-setup-steps.yml @@ -0,0 +1,60 @@ +name: Copilot Setup Steps + +on: + workflow_dispatch: + + push: + branches: + - master + paths: + - .github/workflows/copilot-setup-steps.yml + + pull_request: + paths: + - .github/workflows/copilot-setup-steps.yml + +jobs: + copilot-setup-steps: + runs-on: ubuntu-22.04 + permissions: + contents: read + + steps: + - name: setup workspace + run: | + mkdir -p ${GITHUB_WORKSPACE}/src + + - uses: actions/checkout@v6 + with: + path: src/livox_ros_driver2 + + - name: setup ROS environment + uses: ros-tooling/setup-ros@v0.7 + + - name: install workspace dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake ros-humble-ros-base + source /opt/ros/humble/setup.bash + rosdep update --rosdistro humble + rosdep install --from-paths ${GITHUB_WORKSPACE}/src --ignore-src -r -y + + - name: install Livox SDK2 + run: | + REPO_DIR="/tmp/livox_sdk2_repo" + mkdir -p "$REPO_DIR" + git clone --branch v1.2.5 --depth 1 https://github.com/Livox-SDK/Livox-SDK2.git "$REPO_DIR/Livox-SDK2" + cd "$REPO_DIR/Livox-SDK2" + mkdir -p build && cd build + cmake .. + make -j$(nproc) + sudo make install + sudo ldconfig + rm -rf "$REPO_DIR" + + - name: build workspace + run: | + cd ${GITHUB_WORKSPACE} + source /opt/ros/humble/setup.bash + colcon build --symlink-install --continue-on-error || true + source install/setup.bash diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml new file mode 100644 index 00000000..c9d9d9f8 --- /dev/null +++ b/.github/workflows/ros-ci.yml @@ -0,0 +1,80 @@ +name: ros CI + +on: + push: + # you may want to configure the branches that this should be run on here. + branches: [ "master" ] + pull_request: + +jobs: + test_docker: # On Linux, iterates on all ROS 1 and ROS 2 distributions. + runs-on: ubuntu-latest + permissions: + contents: read + strategy: + matrix: + ros_distribution: + # - noetic + - humble + # - iron + + # Define the Docker image(s) associated with each ROS distribution. + # The include syntax allows additional variables to be defined, like + # docker_image in this case. See documentation: + # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build + # + # Platforms are defined in REP 3 and REP 2000: + # https://ros.org/reps/rep-0003.html + # https://ros.org/reps/rep-2000.html + include: + # Noetic Ninjemys (May 2020 - May 2025) + # - docker_image: ubuntu:focal + # ros_distribution: noetic + # ros_version: 1 + + # Humble Hawksbill (May 2022 - May 2027) + - docker_image: ubuntu:jammy + ros_distribution: humble + ros_version: 2 + + # Iron Irwini (May 2023 - November 2024) + # - docker_image: ubuntu:jammy + # ros_distribution: iron + # ros_version: 2 + + # # Rolling Ridley (No End-Of-Life) + # - docker_image: ubuntu:jammy + # ros_distribution: rolling + # ros_version: 2 + + container: + image: ${{ matrix.docker_image }} + steps: + - uses: actions/checkout@v6 + with: + path: src/livox_ros_driver2 + - name: setup ROS environment + uses: ros-tooling/setup-ros@v0.7 + - name: install build dependencies + run: | + apt-get update + apt-get install -y build-essential cmake + - name: install Livox SDK2 + run: | + REPO_DIR="/tmp/livox_sdk2_repo" + mkdir -p "$REPO_DIR" + git clone --branch v1.2.5 --depth 1 https://github.com/Livox-SDK/Livox-SDK2.git "$REPO_DIR/Livox-SDK2" + cd "$REPO_DIR/Livox-SDK2" + mkdir -p build && cd build + cmake .. + make -j$(nproc) + make install + ldconfig + rm -rf "$REPO_DIR" + - name: build and test ROS 2 + if: ${{ matrix.ros_version == 2 }} + uses: ros-tooling/action-ros-ci@v0.3 + with: + import-token: ${{ github.token }} + target-ros2-distro: ${{ matrix.ros_distribution }} + skip-tests: true diff --git a/.gitignore b/.gitignore index d7c3a849..1ac3a9b9 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ .vscode build -package.xml +#package.xml __pycache__ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 99a8cccf..02a9dda2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,3 +1,24 @@ +# Set defaults if not specified, using ROS environment variables when available +if(NOT DEFINED ROS_EDITION) + if(DEFINED ENV{ROS_VERSION}) + if("$ENV{ROS_VERSION}" STREQUAL "1") + set(ROS_EDITION "ROS1" CACHE STRING "ROS edition to build for (ROS1 or ROS2)" FORCE) + else() + set(ROS_EDITION "ROS2" CACHE STRING "ROS edition to build for (ROS1 or ROS2)" FORCE) + endif() + else() + set(ROS_EDITION "ROS2" CACHE STRING "ROS edition to build for (ROS1 or ROS2)" FORCE) + endif() +endif() + +if(NOT DEFINED HUMBLE_ROS) + if(DEFINED ENV{ROS_DISTRO}) + set(HUMBLE_ROS "$ENV{ROS_DISTRO}" CACHE STRING "ROS2 distro name" FORCE) + else() + set(HUMBLE_ROS "humble" CACHE STRING "ROS2 distro name" FORCE) + endif() +endif() + # judge which cmake codes to use if(ROS_EDITION STREQUAL "ROS1") @@ -317,6 +338,30 @@ else(ROS_EDITION STREQUAL "ROS2") EXECUTABLE ${PROJECT_NAME}_node ) + # livox_set_ip: standalone command-line tool to configure lidar IP addresses + add_executable(livox_set_ip src/tools/livox_set_ip.cpp) + target_include_directories(livox_set_ip PRIVATE + ${LIVOX_LIDAR_SDK_INCLUDE_DIR} + ) + target_link_libraries(livox_set_ip + ${LIVOX_LIDAR_SDK_LIBRARY} + ) + install(TARGETS livox_set_ip + DESTINATION lib/${PROJECT_NAME} + ) + + # livox_scan: standalone command-line tool to discover and report Livox devices + add_executable(livox_scan src/tools/livox_scan.cpp) + target_include_directories(livox_scan PRIVATE + ${LIVOX_LIDAR_SDK_INCLUDE_DIR} + ) + target_link_libraries(livox_scan + ${LIVOX_LIDAR_SDK_LIBRARY} + ) + install(TARGETS livox_scan + DESTINATION lib/${PROJECT_NAME} + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -330,7 +375,7 @@ else(ROS_EDITION STREQUAL "ROS2") ament_auto_package(INSTALL_TO_SHARE config - launch_ROS2 + launch ) endif() \ No newline at end of file diff --git a/README.md b/README.md index ab7c0ad7..55ab5d32 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # Livox ROS Driver 2 -Livox ROS Driver 2 is the 2nd-generation driver package used to connect LiDAR products produced by Livox, applicable for ROS (noetic recommended) and ROS2 (foxy or humble recommended). +Livox ROS Driver 2 is the 2nd-generation driver package used to connect LiDAR products produced by Livox. + +> **Note:** This branch targets **ROS2 only** (Humble or later recommended). ROS1 is not supported. The build system defaults to ROS2 Humble when invoked without arguments via `colcon`. **Note :** @@ -10,31 +12,24 @@ Livox ROS Driver 2 is the 2nd-generation driver package used to connect LiDAR pr ### 1.1 OS requirements - * Ubuntu 18.04 for ROS Melodic; - * Ubuntu 20.04 for ROS Noetic and ROS2 Foxy; - * Ubuntu 22.04 for ROS2 Humble; + * Ubuntu 20.04 for ROS2 Foxy; + * Ubuntu 22.04 for ROS2 Humble (recommended); **Tips:** - Colcon is a build tool used in ROS2. - - How to install colcon: [Colcon installation instructions](https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html) - -### 1.2 Install ROS & ROS2 + Colcon is the build tool used for ROS2. -For ROS Melodic installation, please refer to: -[ROS Melodic installation instructions](https://wiki.ros.org/melodic/Installation) + How to install colcon: [Colcon installation instructions](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html) -For ROS Noetic installation, please refer to: -[ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation) +### 1.2 Install ROS2 For ROS2 Foxy installation, please refer to: -[ROS Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) +[ROS2 Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) For ROS2 Humble installation, please refer to: -[ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) +[ROS2 Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) -Desktop-Full installation is recommend. +Desktop-Full installation is recommended. ## 2. Build & Run Livox ROS Driver 2 @@ -56,45 +51,22 @@ git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ ### 2.3 Build the Livox ROS Driver 2: -#### For ROS (take Noetic as an example): -```shell -source /opt/ros/noetic/setup.sh -./build.sh ROS1 -``` - #### For ROS2 Foxy: ```shell source /opt/ros/foxy/setup.sh -./build.sh ROS2 +colcon build ``` -#### For ROS2 Humble: +#### For ROS2 Humble (recommended): ```shell source /opt/ros/humble/setup.sh -./build.sh humble +colcon build ``` -### 2.4 Run Livox ROS Driver 2: - -#### For ROS: - -```shell -source ../../devel/setup.sh -roslaunch livox_ros_driver2 [launch file] -``` +The build defaults to ROS2 Humble when the `ROS_DISTRO` and `ROS_VERSION` environment variables are set (as they are after sourcing a ROS2 setup script). No additional arguments are required. -in which, - -* **livox_ros_driver2** : is the ROS package name of Livox ROS Driver 2; -* **[launch file]** : is the ROS launch file you want to use; the 'launch_ROS1' folder contains several launch samples for your reference; - -An rviz launch example for HAP LiDAR would be: - -```shell -roslaunch livox_ros_driver2 rviz_HAP.launch -``` +### 2.4 Run Livox ROS Driver 2: -#### For ROS2: ```shell source ../../install/setup.sh ros2 launch livox_ros_driver2 [launch file] @@ -114,18 +86,19 @@ ros2 launch livox_ros_driver2 rviz_HAP_launch.py ### 3.1 Launch file configuration instructions -Launch files of ROS are in the "ws_livox/src/livox_ros_driver2/launch_ROS1" directory and launch files of ROS2 are in the "ws_livox/src/livox_ros_driver2/launch_ROS2" directory. Different launch files have different configuration parameter values and are used in different scenarios: +Launch files are in the `launch/` directory. Different launch files have different configuration parameter values and are used in different scenarios: -| launch file name | Description | -| ------------------------- | ------------------------------------------------------------ | -| rviz_HAP.launch | Connect to HAP LiDAR device
Publish pointcloud2 format data
Autoload rviz | -| msg_HAP.launch | Connect to HAP LiDAR device
Publish livox customized pointcloud data| -| rviz_MID360.launch | Connect to MID360 LiDAR device
Publish pointcloud2 format data
Autoload rviz| -| msg_MID360.launch | Connect to MID360 LiDAR device
Publish livox customized pointcloud data | -| rviz_mixed.launch | Connect to HAP and MID360 LiDAR device
Publish pointcloud2 format data
Autoload rviz| -| msg_mixed.launch | Connect to HAP and MID360 LiDAR device
Publish livox customized pointcloud data | +| Launch file | Description | +| --- | --- | +| `rviz_HAP_launch.py` | Connect to HAP LiDAR device, publish PointCloud2 data, autoload RViz | +| `msg_HAP_launch.py` | Connect to HAP LiDAR device, publish Livox customized pointcloud data | +| `rviz_MID360_launch.py` | Connect to MID360 LiDAR device, publish PointCloud2 data, autoload RViz | +| `msg_MID360_launch.py` | Connect to MID360 LiDAR device, publish Livox customized pointcloud data | +| `rviz_mixed.py` | Connect to HAP and MID360 LiDAR devices, publish PointCloud2 data, autoload RViz | +| `mid360_launch.py` | **Configurable** single MID360 driver — IP, ports, namespace and host IP fully configurable via launch arguments or environment variables; generates config file automatically (see §3.3) | +| `dual_mid360_launch.py` | Launches a **single** driver node that manages both the front and back MID360 lidars in one SDK instance, eliminating cross-instance SDK race conditions; fully configurable via launch arguments or environment variables (see §3.3) | -### 3.2 Livox ros driver 2 internal main parameter configuration instructions +### 3.2 Livox ROS Driver 2 internal main parameter configuration instructions All internal parameters of Livox_ros_driver2 are in the launch file. Below are detailed descriptions of the three commonly used parameters : @@ -133,7 +106,7 @@ All internal parameters of Livox_ros_driver2 are in the launch file. Below are d | ------------ | ------------------------------------------------------------ | ------- | | publish_freq | Set the frequency of point cloud publish
Floating-point data type, recommended values 5.0, 10.0, 20.0, 50.0, etc. The maximum publish frequency is 100.0 Hz.| 10.0 | | multi_topic | If the LiDAR device has an independent topic to publish pointcloud data
0 -- All LiDAR devices use the same topic to publish pointcloud data
1 -- Each LiDAR device has its own topic to publish point cloud data | 0 | -| xfer_format | Set pointcloud format
0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format
1 -- Livox customized pointcloud format
2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0 | +| xfer_format | Set pointcloud format
0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format
1 -- Livox customized pointcloud format
2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (**not supported in ROS2**) | 0 | **Note :** @@ -179,10 +152,135 @@ uint8 tag # livox tag uint8 line # laser number in lidar ``` -3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (only ROS can publish): +3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (**not supported in ROS2**):     Please refer to the pcl :: PointXYZI data structure in the point_types.hpp file of the PCL library. +--- + +### 3.3 Configurable MID360 launch files + +`mid360_launch.py` and `dual_mid360_launch.py` replace the old static-JSON workflow for MID360 sensors. Instead of editing a JSON config file by hand, the LiDAR IP address, host IP, and UDP ports are resolved at launch time from **launch arguments** or **environment variables**, and a temporary config file is generated automatically in `/tmp/`. The temp file is deleted automatically when the launch session ends. + +#### 3.3.1 `mid360_launch.py` — single MID360 + +Launches one MID360 driver node. All connectivity parameters can be supplied via launch arguments or environment variables. Environment variables are only consulted when the corresponding launch argument is left at its default empty value, so launch arguments always take full precedence. + +**Parameter resolution order:** launch argument → environment variable → built-in default + +##### Launch arguments + +| Argument | Env var (no namespace) | Env var (namespace=`NS`) | Default | Description | +| --- | --- | --- | --- | --- | +| `namespace` | — | — | *(empty)* | ROS node/topic namespace **and** env-var prefix (uppercased). Empty = no namespace. | +| `livox_ip` | `LIVOX_IP` | `NS_LIVOX_IP` | `192.168.1.12` | IP address of the MID360 lidar. | +| `livox_host_ip` | `LIVOX_HOST_IP` | `NS_LIVOX_HOST_IP` | *(auto-detected)* | Host NIC IP the lidar sends data to. When empty, auto-detected via kernel routing table. | +| `livox_port_base` | `LIVOX_PORT_BASE` | `NS_LIVOX_PORT_BASE` | `56101` | First UDP host-side listening port. Ports `base … base+4` are used (cmd, push, point, imu, log). | +| `frame_id` | — | — | `` or `livox_frame` | TF frame ID. Defaults to the namespace value when set, otherwise `livox_frame`. | +| `node_name` | — | — | `livox_lidar` | ROS node name. | +| `xfer_format` | — | — | `0` | Point cloud format (0=PointXYZRTLT, 1=custom). | +| `multi_topic` | — | — | `1` | Per-LiDAR topic (1) or shared topic (0). | +| `publish_freq` | — | — | `10.0` | Publish frequency in Hz. | + +##### Port layout + +Given `livox_port_base=N`, the five host UDP ports are assigned as: + +| Channel | Host port | +| --- | --- | +| cmd_data | N | +| push_msg | N+1 | +| point_data | N+2 | +| imu_data | N+3 | +| log_data | N+4 | + +The lidar-side ports (56100, 56200, 56300, 56400, 56500) are fixed by firmware and are always written verbatim into the generated config. + +##### Examples + +```shell +# Minimal — all defaults +ros2 launch livox_ros_driver2 mid360_launch.py + +# Explicit launch arguments +ros2 launch livox_ros_driver2 mid360_launch.py \ + livox_ip:=192.168.1.167 livox_port_base:=56101 + +# Via environment variables (no namespace prefix) +LIVOX_IP=192.168.1.167 ros2 launch livox_ros_driver2 mid360_launch.py + +# With namespace — node appears as /front_lidar/livox_lidar +# and env vars are read with FRONT_LIDAR_ prefix +FRONT_LIDAR_LIVOX_IP=192.168.1.167 \ + ros2 launch livox_ros_driver2 mid360_launch.py namespace:=front_lidar + +# Fully explicit +ros2 launch livox_ros_driver2 mid360_launch.py \ + namespace:=front_lidar \ + livox_ip:=192.168.1.167 \ + livox_host_ip:=192.168.1.10 \ + livox_port_base:=56101 +``` + +--- + +#### 3.3.2 `dual_mid360_launch.py` — two MID360 sensors in a single node + +Launches a **single** `livox_ros_driver2_node` that manages both the **front** and **back** MID360 lidars within one SDK instance. Running both lidars in a single process eliminates the cross-instance SDK race condition: when two separate driver processes each initialise their own Livox SDK, the second SDK's device-discovery broadcasts can interfere with the first lidar's host-registration and interrupt its data stream. A single SDK instance owns both lidars from the start, so no such interference is possible. + +A combined JSON config file is generated at launch time with two per-IP `host_net_info` entries (using the array format required to scope each entry to exactly one lidar) and two `lidar_configs` entries. The file is written to `/tmp/` and removed automatically on shutdown. + +With `multi_topic=1` (default) the driver publishes separate topics for each lidar, remapped to: + +| Lidar | Points topic | IMU topic | TF frame | +| --- | --- | --- | --- | +| Front | `/front_lidar/points` | `/front_lidar/imu` | `front_lidar_link` | +| Back | `/back_lidar/points` | `/back_lidar/imu` | `back_lidar_link` | + +**Default port layout:** + +| Lidar | Ports | +| --- | --- | +| Front | 56101–56105 | +| Back | 56111–56115 | + +##### Launch arguments + +| Argument | Env var | Default | Description | +| --- | --- | --- | --- | +| `front_livox_ip` | `FRONT_LIDAR_LIVOX_IP` | `192.168.1.167` | IP of the front MID360. | +| `back_livox_ip` | `BACK_LIDAR_LIVOX_IP` | `192.168.1.184` | IP of the back MID360. | +| `front_port_base` | `FRONT_LIDAR_LIVOX_PORT_BASE` | `56101` | First UDP host port for the front lidar. | +| `back_port_base` | `BACK_LIDAR_LIVOX_PORT_BASE` | `56111` | First UDP host port for the back lidar. | +| `livox_host_ip` | `LIVOX_HOST_IP` | *(auto-detected)* | Host NIC IP both lidars send data to. Auto-detected via routing table when empty. | +| `front_frame_id` | — | `front_lidar_link` | TF frame ID for front lidar point cloud messages. | +| `back_frame_id` | — | `back_lidar_link` | TF frame ID for back lidar point cloud messages. | +| `connect_timeout_s` | — | `10.0` | Seconds to wait for any lidar to connect before the node shuts down for respawn. | + +##### Examples + +```shell +# Both IPs via environment variables (recommended) +FRONT_LIDAR_LIVOX_IP=192.168.1.167 \ +BACK_LIDAR_LIVOX_IP=192.168.1.184 \ + ros2 launch livox_ros_driver2 dual_mid360_launch.py + +# Override IPs via launch arguments +ros2 launch livox_ros_driver2 dual_mid360_launch.py \ + front_livox_ip:=192.168.1.167 back_livox_ip:=192.168.1.184 + +# Custom port bases (e.g. if 56101 is already in use) +ros2 launch livox_ros_driver2 dual_mid360_launch.py \ + front_port_base:=56201 back_port_base:=56211 + +# Explicit host IP +ros2 launch livox_ros_driver2 dual_mid360_launch.py \ + front_livox_ip:=192.168.1.167 back_livox_ip:=192.168.1.184 \ + livox_host_ip:=192.168.1.10 +``` + +--- + ## 4. LiDAR config LiDAR Configurations (such as ip, port, data type... etc.) can be set via a json-style config file. Config files for single HAP, Mid360 and mixed-LiDARs are in the "config" folder. The parameter naming *'user_config_path'* in launch files indicates such json file path. @@ -330,7 +428,7 @@ For more infomation about the HAP config, please refer to: ] } ``` -3. when multiple nics on the host connect to multiple LiDARs, you need to add objects corresponding to different LiDARs to the lidar_configs array. Run different luanch files separately, and the following is an example of mixing lidar configuration file contents: +3. when multiple nics on the host connect to multiple LiDARs, you need to add objects corresponding to different LiDARs to the lidar_configs array. Run different launch files separately. The following is an example of a mixed LiDAR configuration for reference — note that the launch file examples below use ROS1 XML syntax; for ROS2, equivalent Python launch files should be used instead: **MID360_config1:** ```json @@ -514,19 +612,114 @@ For more infomation about the HAP config, please refer to: ``` -## 5. Supported LiDAR list +## 5. Utility Tools + +### 5.1 livox_set_ip — Change a LiDAR's IP address + +`livox_set_ip` is a standalone command-line tool that uses Livox-SDK2 to assign a new static IP address to any connected Livox LiDAR (e.g. MID-360, HAP). It is useful for initial device setup or when you need to move a LiDAR to a different subnet. + +**Usage:** + +```shell +ros2 run livox_ros_driver2 livox_set_ip [host_ip] +``` + +| Argument | Description | +| --- | --- | +| `current_lidar_ip` | Current IP of the LiDAR to configure | +| `new_ip` | New static IP to assign to the LiDAR | +| `netmask` | Subnet mask (e.g. `255.255.255.0`) | +| `gateway` | Gateway address (e.g. `192.168.1.1`) | +| `host_ip` | *(optional)* IP of the host NIC connected to the LiDAR; omit to let the SDK auto-detect | + +**Examples:** + +```shell +# Change lidar IP; host NIC is explicitly specified +ros2 run livox_ros_driver2 livox_set_ip 192.168.1.100 192.168.1.167 255.255.255.0 192.168.1.1 192.168.1.10 + +# Host IP auto-detected by the SDK +ros2 run livox_ros_driver2 livox_set_ip 192.168.1.167 192.168.1.184 255.255.255.0 192.168.1.1 +``` + +> **IMPORTANT:** Power-cycle the LiDAR after the tool reports success for the new IP address to take effect. + +### 5.2 livox_scan — Discover LiDAR devices on the network + +`livox_scan` listens for Livox device announcements for a configurable period and prints a summary table of every device found, including its IP address, device type, and serial number. + +**Usage:** + +```shell +ros2 run livox_ros_driver2 livox_scan [OPTIONS] [host_ip [timeout_seconds]] +``` + +**Options:** + +| Option | Description | +| --- | --- | +| `-h`, `--help` | Show help message and exit | + +**Arguments:** + +| Argument | Description | +| --- | --- | +| `host_ip` | *(optional)* IP of the host NIC facing the LiDARs; omit to let the SDK auto-detect | +| `timeout_seconds` | *(optional)* How long to scan in seconds (default: `10`) | + +**Examples:** + +```shell +# Show help +ros2 run livox_ros_driver2 livox_scan --help + +# Scan all NICs for 10 seconds (default) +ros2 run livox_ros_driver2 livox_scan + +# Scan from a specific NIC +ros2 run livox_ros_driver2 livox_scan 192.168.1.10 + +# Scan from a specific NIC with a 5-second timeout +ros2 run livox_ros_driver2 livox_scan 192.168.1.10 5 +``` + +**Example output:** + +``` +=== Livox LiDAR scanner === + Host NIC : (auto-detect) + Timeout : 10 s + +[info] Scanning for 10 second(s) ... + +[found] 192.168.1.167 type=Mid-360 sn=0TFDH7600601234 +[found] 192.168.1.184 type=Mid-360 sn=0TFDH7600605678 + +[result] Found 2 device(s): + ++------------------+------------------+--------------------+ +| IP Address | Device Type | Serial Number | ++------------------+------------------+--------------------+ +| 192.168.1.167 | Mid-360 | 0TFDH7600601234 | +| 192.168.1.184 | Mid-360 | 0TFDH7600605678 | ++------------------+------------------+--------------------+ +``` + +--- + +## 6. Supported LiDAR list * HAP * Mid360 * (more types are comming soon...) -## 6. FAQ +## 7. FAQ -### 6.1 launch with "livox_lidar_rviz_HAP.launch" but no point cloud display on the grid? +### 7.1 launch with "livox_lidar_rviz_HAP.launch" but no point cloud display on the grid? Please check the "Global Options - Fixed Frame" field in the RViz "Display" pannel. Set the field value to "livox_frame" and check the "PointCloud2" option in the pannel. -### 6.2 launch with command "ros2 launch livox_lidar_rviz_HAP_launch.py" but cannot open shared object file "liblivox_sdk_shared.so" ? +### 7.2 launch with command "ros2 launch livox_lidar_rviz_HAP_launch.py" but cannot open shared object file "liblivox_sdk_shared.so" ? Please add '/usr/local/lib' to the env LD_LIBRARY_PATH. diff --git a/config/MID360_config_167.json b/config/MID360_config_167.json new file mode 100644 index 00000000..c42b0d74 --- /dev/null +++ b/config/MID360_config_167.json @@ -0,0 +1,41 @@ +{ + "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.10", + "cmd_data_port": 56101, + "push_msg_ip": "192.168.1.10", + "push_msg_port": 56201, + "point_data_ip": "192.168.1.10", + "point_data_port": 56301, + "imu_data_ip" : "192.168.1.10", + "imu_data_port": 56401, + "log_data_ip" : "", + "log_data_port": 56501 + } + }, + "lidar_configs" : [ + { + "ip" : "192.168.1.167", + "pcl_data_type" : 1, + "pattern_mode" : 0, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} \ No newline at end of file diff --git a/config/MID360_config_184.json b/config/MID360_config_184.json new file mode 100644 index 00000000..d0af6cd6 --- /dev/null +++ b/config/MID360_config_184.json @@ -0,0 +1,41 @@ +{ + "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.10", + "cmd_data_port": 56102, + "push_msg_ip": "192.168.1.10", + "push_msg_port": 56202, + "point_data_ip": "192.168.1.10", + "point_data_port": 56302, + "imu_data_ip" : "192.168.1.10", + "imu_data_port": 56402, + "log_data_ip" : "", + "log_data_port": 56502 + } + }, + "lidar_configs" : [ + { + "ip" : "192.168.1.184", + "pcl_data_type" : 1, + "pattern_mode" : 0, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} \ No newline at end of file diff --git a/launch/dual_mid360_launch.py b/launch/dual_mid360_launch.py new file mode 100644 index 00000000..1a49187b --- /dev/null +++ b/launch/dual_mid360_launch.py @@ -0,0 +1,342 @@ +""" +dual_mid360_launch.py — Launch a SINGLE livox_ros_driver2 node that manages +BOTH MID360 lidars (front and back). + +Having a single SDK instance handle both lidars eliminates the race condition +that occurs when two separate driver processes each run their own SDK: the +second SDK's device-discovery broadcasts can interfere with the first lidar's +host-registration, interrupting its data stream. With one process owning both +lidars no cross-instance interference is possible. + +Port layout (5 consecutive ports per lidar, 10-port gap between the two): + front_lidar : 56101 – 56105 + back_lidar : 56111 – 56115 +""" + +import json +import os +import socket +import tempfile + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler +from launch.event_handlers import OnShutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 1 # 1-One LiDAR one topic +data_src = 0 # 0-lidar +publish_freq = 10.0 # Hz: 5.0, 10.0, 20.0, 50.0 +output_type = 0 +################### user configure parameters for ros2 end ##################### + +# --------------------------------------------------------------------------- +# Defaults +# --------------------------------------------------------------------------- +DEFAULT_FRONT_IP = '192.168.1.167' +DEFAULT_BACK_IP = '192.168.1.184' +DEFAULT_FRONT_PORT_BASE = 56101 # ports: 56101 – 56105 +DEFAULT_BACK_PORT_BASE = 56111 # ports: 56111 – 56115 +DEFAULT_FRONT_FRAME_ID = 'front_lidar_link' +DEFAULT_BACK_FRAME_ID = 'back_lidar_link' + + +def _detect_host_ip(target_ip: str) -> str: + """Return the local IP address that the kernel would use to reach *target_ip*.""" + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: + s.connect((target_ip, 56100)) # port is irrelevant; no packet is sent + return s.getsockname()[0] + + +def _generate_dual_config( + front_ip: str, back_ip: str, + host_ip: str, + front_port_base: int, back_port_base: int, +) -> str: + """Write a MID360 JSON config for BOTH lidars to a temp file and return its path. + + Using ``host_net_info`` as a JSON array with per-IP entries is critical: + it routes each entry into the SDK's ``custom_lidars_cfg_map_`` (keyed by + the specific lidar IP) rather than the ``type_lidars_cfg_map_`` (keyed by + device type = "all MID360s"). The type-keyed map applies the same host + registration to EVERY discovered MID360, which would prevent the second + lidar from being registered to its own port range. + """ + config = { + 'lidar_summary_info': {'lidar_type': 8}, + 'MID360': { + 'lidar_net_info': { # fixed by firmware, not configurable + 'cmd_data_port': 56100, + 'push_msg_port': 56200, + 'point_data_port': 56300, + 'imu_data_port': 56400, + 'log_data_port': 56500, + }, + 'host_net_info': [ # array format → per-IP entries + { + 'lidar_ip': [front_ip], + 'host_ip': host_ip, + 'cmd_data_port': front_port_base, + 'push_msg_port': front_port_base + 1, + 'point_data_port': front_port_base + 2, + 'imu_data_port': front_port_base + 3, + 'log_data_port': front_port_base + 4, + }, + { + 'lidar_ip': [back_ip], + 'host_ip': host_ip, + 'cmd_data_port': back_port_base, + 'push_msg_port': back_port_base + 1, + 'point_data_port': back_port_base + 2, + 'imu_data_port': back_port_base + 3, + 'log_data_port': back_port_base + 4, + }, + ], + }, + 'lidar_configs': [ + { + 'ip': front_ip, + 'pcl_data_type': 1, + 'pattern_mode': 0, + 'extrinsic_parameter': { + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, + 'x': 0, 'y': 0, 'z': 0, + }, + }, + { + 'ip': back_ip, + 'pcl_data_type': 1, + 'pattern_mode': 0, + 'extrinsic_parameter': { + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, + 'x': 0, 'y': 0, 'z': 0, + }, + }, + ], + } + + tmp = tempfile.NamedTemporaryFile( + mode='w', + prefix='mid360_dual_config', + suffix='.json', + delete=False, + ) + json.dump(config, tmp, indent=2) + tmp.flush() + tmp.close() + return tmp.name + + +# --------------------------------------------------------------------------- +# OpaqueFunction: executed at launch-time with fully-resolved argument values +# --------------------------------------------------------------------------- + +def _launch_setup(context, *args, **kwargs): + """Resolve all parameters, generate the combined config file, and return + a single driver Node that manages both lidars.""" + + front_ip = LaunchConfiguration('front_livox_ip').perform(context) or \ + os.environ.get('FRONT_LIDAR_LIVOX_IP', DEFAULT_FRONT_IP) + back_ip = LaunchConfiguration('back_livox_ip').perform(context) or \ + os.environ.get('BACK_LIDAR_LIVOX_IP', DEFAULT_BACK_IP) + + host_ip_arg = LaunchConfiguration('livox_host_ip').perform(context) or \ + os.environ.get('LIVOX_HOST_IP', '') + if host_ip_arg: + host_ip = host_ip_arg + else: + host_ip = _detect_host_ip(front_ip) + print( + f'[dual_mid360_launch] Auto-detected host IP {host_ip} ' + f'to reach front lidar {front_ip}. ' + f'Override with livox_host_ip arg or LIVOX_HOST_IP env var.' + ) + + front_port_base = int( + LaunchConfiguration('front_port_base').perform(context) or + os.environ.get('FRONT_LIDAR_LIVOX_PORT_BASE', str(DEFAULT_FRONT_PORT_BASE)) + ) + back_port_base = int( + LaunchConfiguration('back_port_base').perform(context) or + os.environ.get('BACK_LIDAR_LIVOX_PORT_BASE', str(DEFAULT_BACK_PORT_BASE)) + ) + + front_frame_id = LaunchConfiguration('front_frame_id').perform(context) or DEFAULT_FRONT_FRAME_ID + back_frame_id = LaunchConfiguration('back_frame_id').perform(context) or DEFAULT_BACK_FRAME_ID + + config_path = _generate_dual_config( + front_ip, back_ip, host_ip, front_port_base, back_port_base + ) + + print( + f'[dual_mid360_launch] Single driver node managing both lidars:\n' + f' front: ip={front_ip} ports={front_port_base}–{front_port_base + 4}' + f' frame_id={front_frame_id!r}\n' + f' back: ip={back_ip} ports={back_port_base}–{back_port_base + 4}' + f' frame_id={back_frame_id!r}\n' + f' host_ip={host_ip} config={config_path}' + ) + + # Build topic remappings for both lidars. + # With multi_topic=1 the driver publishes on livox/lidar_ and + # livox/imu_ where dots in the IP are replaced by underscores. + front_suffix = front_ip.replace('.', '_') + back_suffix = back_ip.replace('.', '_') + + lidar_node = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_dual', + output='screen', + respawn=True, + parameters=[ + {'xfer_format': int(LaunchConfiguration('xfer_format').perform(context))}, + {'multi_topic': int(LaunchConfiguration('multi_topic').perform(context))}, + {'data_src': int(LaunchConfiguration('data_src').perform(context))}, + {'publish_freq': float(LaunchConfiguration('publish_freq').perform(context))}, + {'output_data_type': int(LaunchConfiguration('output_type').perform(context))}, + {'frame_id': front_frame_id}, + {'user_config_path': config_path}, + {'cmdline_input_bd_code': ''}, + {'connect_timeout_s': float(LaunchConfiguration('connect_timeout_s').perform(context))}, + ], + remappings=[ + # front lidar + (f'livox/lidar_{front_suffix}', 'front_lidar/points'), + (f'livox/imu_{front_suffix}', 'front_lidar/imu'), + # back lidar + (f'livox/lidar_{back_suffix}', 'back_lidar/points'), + (f'livox/imu_{back_suffix}', 'back_lidar/imu'), + ], + ) + + # Cleanup: delete the temp config file on shutdown + def _delete_config(_event, _context): + try: + os.remove(config_path) + print(f'[dual_mid360_launch] Removed temp config: {config_path}') + except OSError: + pass + + cleanup_handler = RegisterEventHandler( + OnShutdown(on_shutdown=_delete_config) + ) + + return [lidar_node, cleanup_handler] + + +# --------------------------------------------------------------------------- +# generate_launch_description +# --------------------------------------------------------------------------- + +def generate_launch_description(): + return LaunchDescription([ + # --- driver parameters ----------------------------------------------- + DeclareLaunchArgument( + 'xfer_format', + default_value=str(xfer_format), + description='0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format', + ), + DeclareLaunchArgument( + 'multi_topic', + default_value=str(multi_topic), + description='0-Multilidar topic, 1-One LiDAR one topic', + ), + DeclareLaunchArgument( + 'data_src', + default_value=str(data_src), + description='0-lidar', + ), + DeclareLaunchArgument( + 'publish_freq', + default_value=str(publish_freq), + description='Publish frequency in Hz (e.g. 5.0, 10.0, 20.0, 50.0)', + ), + DeclareLaunchArgument( + 'output_type', + default_value=str(output_type), + description='Output data type', + ), + DeclareLaunchArgument( + 'connect_timeout_s', + default_value='10.0', + description=( + 'Seconds to wait for any lidar to connect after SDK init. ' + 'If no callback is received within this time the node shuts ' + 'down (and is respawned by the launch system).' + ), + ), + + # --- front lidar connectivity ---------------------------------------- + DeclareLaunchArgument( + 'front_livox_ip', + default_value=os.environ.get('FRONT_LIDAR_LIVOX_IP', DEFAULT_FRONT_IP), + description=( + f'IP of the front MID360 lidar. ' + f'Also overridable via FRONT_LIDAR_LIVOX_IP env var. ' + f'Default: {DEFAULT_FRONT_IP}' + ), + ), + DeclareLaunchArgument( + 'front_port_base', + default_value=os.environ.get('FRONT_LIDAR_LIVOX_PORT_BASE', str(DEFAULT_FRONT_PORT_BASE)), + description=( + f'First UDP host port for the front lidar (uses base…base+4). ' + f'Also overridable via FRONT_LIDAR_LIVOX_PORT_BASE env var. ' + f'Default: {DEFAULT_FRONT_PORT_BASE}–{DEFAULT_FRONT_PORT_BASE + 4}.' + ), + ), + DeclareLaunchArgument( + 'front_frame_id', + default_value=DEFAULT_FRONT_FRAME_ID, + description=( + f'TF frame ID for the front lidar point cloud messages. ' + f'Default: {DEFAULT_FRONT_FRAME_ID}' + ), + ), + + # --- back lidar connectivity ------------------------------------------ + DeclareLaunchArgument( + 'back_livox_ip', + default_value=os.environ.get('BACK_LIDAR_LIVOX_IP', DEFAULT_BACK_IP), + description=( + f'IP of the back MID360 lidar. ' + f'Also overridable via BACK_LIDAR_LIVOX_IP env var. ' + f'Default: {DEFAULT_BACK_IP}' + ), + ), + DeclareLaunchArgument( + 'back_port_base', + default_value=os.environ.get('BACK_LIDAR_LIVOX_PORT_BASE', str(DEFAULT_BACK_PORT_BASE)), + description=( + f'First UDP host port for the back lidar (uses base…base+4). ' + f'Also overridable via BACK_LIDAR_LIVOX_PORT_BASE env var. ' + f'Default: {DEFAULT_BACK_PORT_BASE}–{DEFAULT_BACK_PORT_BASE + 4}.' + ), + ), + DeclareLaunchArgument( + 'back_frame_id', + default_value=DEFAULT_BACK_FRAME_ID, + description=( + f'TF frame ID for the back lidar point cloud messages. ' + f'Default: {DEFAULT_BACK_FRAME_ID}' + ), + ), + + # --- shared host connectivity ----------------------------------------- + DeclareLaunchArgument( + 'livox_host_ip', + default_value=os.environ.get('LIVOX_HOST_IP', ''), + description=( + 'Host IP that both lidars send data to. ' + 'Overrides env var LIVOX_HOST_IP. ' + 'When empty, auto-detected via routing table using front lidar IP.' + ), + ), + + # --- dynamic setup via OpaqueFunction -------------------------------- + OpaqueFunction(function=_launch_setup), + ]) diff --git a/launch/mid360_launch.py b/launch/mid360_launch.py new file mode 100644 index 00000000..eb8633d8 --- /dev/null +++ b/launch/mid360_launch.py @@ -0,0 +1,343 @@ +import json +import os +import socket +import tempfile + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler +from launch.event_handlers import OnShutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +################### user configure parameters for ros2 start ################### +xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 1 # 1-One LiDAR one topic +data_src = 0 # 0-lidar +publish_freq = 10.0 # Hz: 5.0, 10.0, 20.0, 50.0 +output_type = 0 +################### user configure parameters for ros2 end ##################### + +# --------------------------------------------------------------------------- +# Defaults used when neither a launch argument nor an environment variable +# provides a value. +# --------------------------------------------------------------------------- +DEFAULT_LIVOX_IP = '192.168.1.12' +DEFAULT_LIVOX_PORT_BASE = 56101 # host_net_info ports: base, base+1, …, base+4 + + +def _env_var(namespace: str, base_name: str) -> str: + """Return the fully-qualified environment variable name for *base_name*. + + When *namespace* is non-empty the variable is ``_``; + when it is empty the variable is just ````. + """ + ns = namespace.strip().upper() + return f'{ns}_{base_name}' if ns else base_name + + +def _resolve(launch_arg_value: str, env_var_name: str, default: str) -> str: + """Return the effective value using the precedence: + + launch argument > environment variable > default + """ + # An unset DeclareLaunchArgument leaves its LaunchConfiguration at the + # declared default_value, which we set to the empty sentinel ''. + if launch_arg_value != '': + return launch_arg_value + return os.environ.get(env_var_name, default) + + +def _detect_host_ip(target_ip: str) -> str: + """Return the local IP address that the kernel would use to reach *target_ip*.""" + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: + s.connect((target_ip, 56100)) # port is irrelevant; no packet is sent + return s.getsockname()[0] + + +def _generate_config(livox_ip: str, host_ip: str, port_base: int, namespace: str = '') -> str: + """Write a MID360 JSON config to a temporary file and return its path. + + The five host UDP listening ports are assigned as: + cmd_data_port = port_base + push_msg_port = port_base + 1 + point_data_port = port_base + 2 + imu_data_port = port_base + 3 + log_data_port = port_base + 4 + + The lidar-side ports (56100, 56200, …) are fixed by firmware and + are therefore kept at their standard values. + """ + # IMPORTANT: host_net_info MUST be a JSON array whose entries each carry a + # "lidar_ip" array field. This routes the entry into the SDK's + # `custom_lidars_cfg_map_` (keyed by the specific lidar IP) instead of the + # `type_lidars_cfg_map_` (keyed by device-type = "all MID360s"). + # When host_net_info is a plain JSON object the SDK stores the entry in + # type_lidars_cfg_map_, which it then applies to EVERY MID360 it discovers + # on the subnet – including lidars owned by other driver instances. That + # overwrites their host-IP/port registration and kills their data stream. + config = { + 'lidar_summary_info': {'lidar_type': 8}, + 'MID360': { + 'lidar_net_info': { # fixed by firmware, not configurable + 'cmd_data_port': 56100, + 'push_msg_port': 56200, + 'point_data_port': 56300, + 'imu_data_port': 56400, + 'log_data_port': 56500, + }, + 'host_net_info': [ # array format → per-IP entry, not generic device-type entry + { + 'lidar_ip': [livox_ip], # binds this entry to exactly this lidar + 'host_ip': host_ip, + 'cmd_data_port': port_base, + 'push_msg_port': port_base + 1, + 'point_data_port': port_base + 2, + 'imu_data_port': port_base + 3, + 'log_data_port': port_base + 4, + } + ], + }, + 'lidar_configs': [ + { + 'ip': livox_ip, + 'pcl_data_type': 1, + 'pattern_mode': 0, + 'extrinsic_parameter': { + 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, + 'x': 0, 'y': 0, 'z': 0, + }, + } + ], + } + + # NamedTemporaryFile with delete=False so the driver process can open it + # after this function returns. The file lives for the duration of the + # launch session; it is cleaned up by the OS on reboot at the latest. + ns_part = f'{namespace}_' if namespace else '' + tmp = tempfile.NamedTemporaryFile( + mode='w', + prefix=f'mid360_{ns_part}config', + suffix='.json', + delete=False, + ) + json.dump(config, tmp, indent=2) + tmp.flush() + tmp.close() + return tmp.name + + +# --------------------------------------------------------------------------- +# OpaqueFunction: executed at launch-time with fully-resolved argument values +# --------------------------------------------------------------------------- + +def _launch_setup(context, *args, **kwargs): + """Resolve all parameters, generate the config file, and return the Node.""" + + # --- namespace ----------------------------------------------------------- + namespace = LaunchConfiguration('namespace').perform(context).strip() + + # --- LivoxIp ------------------------------------------------------------- + livox_ip = _resolve( + LaunchConfiguration('livox_ip').perform(context), + _env_var(namespace, 'LIVOX_IP'), + DEFAULT_LIVOX_IP, + ) + + # --- LivoxHostIp --------------------------------------------------------- + host_ip_arg = _resolve( + LaunchConfiguration('livox_host_ip').perform(context), + _env_var(namespace, 'LIVOX_HOST_IP'), + '', # empty → auto-detect + ) + if host_ip_arg: + host_ip = host_ip_arg + else: + host_ip = _detect_host_ip(livox_ip) + print( + f'[mid360_launch] Auto-detected host IP {host_ip} to reach lidar IP {livox_ip}. ' + f'You can override this by setting the launch argument or environment variable for livox_host_ip.' + ) + + # --- LivoxPortBase ------------------------------------------------------- + port_base_str = _resolve( + LaunchConfiguration('livox_port_base').perform(context), + _env_var(namespace, 'LIVOX_PORT_BASE'), + str(DEFAULT_LIVOX_PORT_BASE), + ) + port_base = int(port_base_str) + + # --- Generate config file ------------------------------------------------ + config_path = _generate_config(livox_ip, host_ip, port_base, namespace) + + # --- frame_id ------------------------------------------------------------ + frame_id_arg = LaunchConfiguration('frame_id').perform(context) + if frame_id_arg: + frame_id = frame_id_arg + else: + frame_id = f"{namespace}_link" if namespace else 'livox_frame' + + print( + f'[mid360_launch] namespace={namespace!r} ' + f'livox_ip={livox_ip} host_ip={host_ip} ' + f'port_base={port_base} frame_id={frame_id!r} config={config_path}' + ) + + # --- Node ---------------------------------------------------------------- + node_name = LaunchConfiguration('node_name').perform(context) or 'livox_lidar' + ns_arg = namespace if namespace else '' + + # Build topic remappings: the driver appends the IP (dots→underscores) to + # the topic names, e.g. livox/lidar_192_168_1_184. Remap these to clean + # names: with a namespace → "points" / "imu" (namespace is already the + # prefix); without a namespace → "/points" / "/imu". + ip_suffix = livox_ip.replace('.', '_') + if namespace: + points_dst = 'points' + imu_dst = 'imu' + else: + points_dst = f'{node_name}/points' + imu_dst = f'{node_name}/imu' + + lidar_node = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name=node_name, + namespace=ns_arg, + output='screen', + respawn=True, + parameters=[ + {'xfer_format': int(LaunchConfiguration('xfer_format').perform(context))}, + {'multi_topic': int(LaunchConfiguration('multi_topic').perform(context))}, + {'data_src': int(LaunchConfiguration('data_src').perform(context))}, + {'publish_freq': float(LaunchConfiguration('publish_freq').perform(context))}, + {'output_data_type': int(LaunchConfiguration('output_type').perform(context))}, + {'frame_id': frame_id}, + {'user_config_path': config_path}, + {'cmdline_input_bd_code': ''}, + {'connect_timeout_s': float(LaunchConfiguration('connect_timeout_s').perform(context))}, + ], + remappings=[ + (f'livox/lidar_{ip_suffix}', points_dst), + (f'livox/imu_{ip_suffix}', imu_dst), + ], + ) + + # --- Cleanup: delete the temp config file on shutdown ---------------- + def _delete_config(_event, _context): + try: + os.remove(config_path) + print(f'[mid360_launch] Removed temp config: {config_path}') + except OSError: + pass + + cleanup_handler = RegisterEventHandler( + OnShutdown(on_shutdown=_delete_config) + ) + + return [lidar_node, cleanup_handler] + + +# --------------------------------------------------------------------------- +# generate_launch_description +# --------------------------------------------------------------------------- + +def generate_launch_description(): + return LaunchDescription([ + # --- driver parameters ----------------------------------------------- + DeclareLaunchArgument( + 'xfer_format', + default_value=str(xfer_format), + description='0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format', + ), + DeclareLaunchArgument( + 'multi_topic', + default_value=str(multi_topic), + description='0-Multilidar topic, 1-One LiDAR one topic', + ), + DeclareLaunchArgument( + 'data_src', + default_value=str(data_src), + description='0-lidar', + ), + DeclareLaunchArgument( + 'publish_freq', + default_value=str(publish_freq), + description='Publish frequency in Hz (e.g. 5.0, 10.0, 20.0, 50.0)', + ), + DeclareLaunchArgument( + 'output_type', + default_value=str(output_type), + description='Output data type', + ), + DeclareLaunchArgument( + 'frame_id', + default_value='', + description=( + 'TF frame ID for the lidar. ' + 'When empty (default): uses "_livox_frame" if namespace is set, ' + 'otherwise "livox_frame".' + ), + ), + DeclareLaunchArgument( + 'node_name', + default_value='livox_lidar', + description='ROS node name for the driver instance', + ), + DeclareLaunchArgument( + 'connect_timeout_s', + default_value='5.0', + description=( + 'Seconds to wait for any lidar to respond after SDK init. ' + 'If no callback is received within this time the node shuts ' + 'down (and is respawned by the launch system).' + ), + ), + + # --- namespace / connectivity ---------------------------------------- + DeclareLaunchArgument( + 'namespace', + default_value='', + description=( + 'Node/topic namespace AND prefix for environment-variable lookups. ' + 'When empty (default): no namespace is applied to the node/topics and ' + 'env vars are read without a prefix: ' + 'LIVOX_IP, LIVOX_HOST_IP, LIVOX_PORT_BASE. ' + 'When set to e.g. "front_lidar": the node and all topics are placed under ' + 'that namespace and env vars are read with the uppercased prefix: ' + 'FRONT_LIDAR_LIVOX_IP, FRONT_LIDAR_LIVOX_HOST_IP, FRONT_LIDAR_LIVOX_PORT_BASE. ' + 'Launch arguments always take precedence over environment variables.' + ), + ), + DeclareLaunchArgument( + 'livox_ip', + default_value='', + description=( + 'IP address of the MID360 lidar. ' + 'Overrides env var [_]LIVOX_IP. ' + f'Fallback default: {DEFAULT_LIVOX_IP}' + ), + ), + DeclareLaunchArgument( + 'livox_host_ip', + default_value='', + description=( + 'Host IP that the lidar sends data to. ' + 'Overrides env var [_]LIVOX_HOST_IP. ' + 'When empty, auto-detected via routing table.' + ), + ), + DeclareLaunchArgument( + 'livox_port_base', + default_value='', + description=( + 'First UDP port the host listens on (host_net_info). ' + 'Overrides env var [_]LIVOX_PORT_BASE. ' + f'Ports are base, base+1, …, base+4. ' + f'Fallback default: {DEFAULT_LIVOX_PORT_BASE}' + ), + ), + + # --- dynamic setup via OpaqueFunction -------------------------------- + OpaqueFunction(function=_launch_setup), + ]) diff --git a/launch_ROS2/msg_HAP_launch.py b/launch/msg_HAP_launch.py similarity index 100% rename from launch_ROS2/msg_HAP_launch.py rename to launch/msg_HAP_launch.py diff --git a/launch_ROS2/msg_MID360_launch.py b/launch/msg_MID360_launch.py similarity index 100% rename from launch_ROS2/msg_MID360_launch.py rename to launch/msg_MID360_launch.py diff --git a/launch_ROS2/rviz_HAP_launch.py b/launch/rviz_HAP_launch.py similarity index 100% rename from launch_ROS2/rviz_HAP_launch.py rename to launch/rviz_HAP_launch.py diff --git a/launch_ROS2/rviz_MID360_launch.py b/launch/rviz_MID360_launch.py similarity index 100% rename from launch_ROS2/rviz_MID360_launch.py rename to launch/rviz_MID360_launch.py diff --git a/launch_ROS2/rviz_mixed.py b/launch/rviz_mixed.py similarity index 100% rename from launch_ROS2/rviz_mixed.py rename to launch/rviz_mixed.py diff --git a/launch_ROS1/msg_HAP.launch b/launch_ROS1/msg_HAP.launch deleted file mode 100644 index 005e038e..00000000 --- a/launch_ROS1/msg_HAP.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/msg_MID360.launch b/launch_ROS1/msg_MID360.launch deleted file mode 100644 index 1e087455..00000000 --- a/launch_ROS1/msg_MID360.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/msg_mixed.launch b/launch_ROS1/msg_mixed.launch deleted file mode 100644 index bd4fe79a..00000000 --- a/launch_ROS1/msg_mixed.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/rviz_HAP.launch b/launch_ROS1/rviz_HAP.launch deleted file mode 100644 index 572098ae..00000000 --- a/launch_ROS1/rviz_HAP.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/rviz_MID360.launch b/launch_ROS1/rviz_MID360.launch deleted file mode 100644 index 44c8bff7..00000000 --- a/launch_ROS1/rviz_MID360.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch_ROS1/rviz_mixed.launch b/launch_ROS1/rviz_mixed.launch deleted file mode 100644 index d1851e82..00000000 --- a/launch_ROS1/rviz_mixed.launch +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..96f57623 --- /dev/null +++ b/package.xml @@ -0,0 +1,35 @@ + + + + livox_ros_driver2 + 1.0.0 + The ROS device driver for Livox 3D LiDARs, for ROS2 + feng + MIT + + ament_cmake_auto + rosidl_default_generators + rosidl_interface_packages + + rclcpp + rclcpp_components + std_msgs + sensor_msgs + rcutils + pcl_conversions + rcl_interfaces + libpcl-all-dev + + rosbag2 + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + git + apr + + + ament_cmake + + diff --git a/src/call_back/lidar_common_callback.cpp b/src/call_back/lidar_common_callback.cpp index 43efb247..afdefc5a 100644 --- a/src/call_back/lidar_common_callback.cpp +++ b/src/call_back/lidar_common_callback.cpp @@ -25,6 +25,7 @@ #include "lidar_common_callback.h" #include "../lds_lidar.h" +#include "../include/livox_log.h" #include @@ -32,34 +33,31 @@ namespace livox_ros { void LidarCommonCallback::OnLidarPointClounCb(PointFrame* frame, void* client_data) { if (frame == nullptr) { - printf("LidarPointCloudCb frame is nullptr.\n"); + LIVOX_ERROR("OnLidarPointCloudCb: frame is nullptr"); return; } if (client_data == nullptr) { - printf("Lidar point cloud cb failed, client data is nullptr.\n"); + LIVOX_ERROR("OnLidarPointCloudCb: client data is nullptr"); return; } - if (frame->lidar_num ==0) { - printf("LidarPointCloudCb lidar_num:%u.\n", frame->lidar_num); + if (frame->lidar_num == 0) { + LIVOX_WARN("OnLidarPointCloudCb: lidar_num is 0"); return; } LdsLidar *lds_lidar = static_cast(client_data); - - //printf("Lidar point cloud, lidar_num:%u.\n", frame->lidar_num); - lds_lidar->StoragePointData(frame); } void LidarCommonCallback::LidarImuDataCallback(ImuData* imu_data, void *client_data) { if (imu_data == nullptr) { - printf("Imu data is nullptr.\n"); + LIVOX_ERROR("LidarImuDataCallback: imu_data is nullptr"); return; } if (client_data == nullptr) { - printf("Lidar point cloud cb failed, client data is nullptr.\n"); + LIVOX_ERROR("LidarImuDataCallback: client data is nullptr"); return; } diff --git a/src/call_back/livox_lidar_callback.cpp b/src/call_back/livox_lidar_callback.cpp index 60bb237f..2c60c85c 100644 --- a/src/call_back/livox_lidar_callback.cpp +++ b/src/call_back/livox_lidar_callback.cpp @@ -26,66 +26,162 @@ #include #include -#include +#include +#include + +#include "../include/livox_log.h" namespace livox_ros { +// --------------------------------------------------------------------------- +// Race-condition analysis: dual-lidar initialisation +// --------------------------------------------------------------------------- +// +// RACE 1 — connect_state read/write without synchronisation (FIXED) +// connect_state is written by SDK callback threads (inside config_mutex_) but +// was previously read by the polling threads in lddc.cpp without any lock. +// `volatile` prevents compiler optimisation but does NOT prevent CPU +// reordering; a stale value could be observed indefinitely on some +// architectures. Fixed by changing connect_state to std::atomic<> in +// comm.h, so every write is immediately visible to all readers. +// +// RACE 2 — LidarInfoChangeCallback re-fires while config is in flight (FIXED) +// The Livox SDK re-fires LidarInfoChangeCallback whenever it re-discovers a +// lidar (e.g. after a brief network hiccup or on a periodic poll). Each +// invocation ORs new bits into set_bits and dispatches fresh SDK commands to +// the lidar. If a re-fire happens before all ACKs from the first fire have +// arrived, set_bits accumulates extra bits. More critically, the fresh +// SetLivoxLidarPclDataType / SetLivoxLidarScanPattern commands cause the +// lidar to interrupt streaming while it applies the new config — and if any +// of those second-round ACKs are lost, set_bits never reaches 0 and +// connect_state never transitions to kConnectStateSampling, so no point +// cloud data is ever published. +// +// Fix: check connect_state at the start of the callback; if the lidar is +// already Sampling, skip all config commands and only refresh work mode. +// This prevents any re-fire from restarting the config handshake. +// +// RACE 3 — cross-instance SDK interference (PARTIALLY MITIGATED) +// When dual_mid360_launch.py starts the second driver instance (after the +// configured delay), that instance's Livox SDK initialises and discovers +// ALL reachable lidars on the network — including the front lidar that +// belongs to the first instance. The application-level AllowHandle guard +// prevents our *application* code from sending commands to foreign lidars, +// but the Livox SDK itself may send lower-level discovery/handshake packets +// (e.g. GetDeviceInfo) to all discovered devices. These packets can cause +// the front lidar to briefly re-negotiate its "host" endpoint, interrupting +// the first driver's data stream. +// +// Mitigations already in place: +// * The back-lidar start delay (back_lidar_delay, default 5 s) lets the +// front lidar fully handshake before the second SDK starts. +// * The config JSON files specify explicit host IPs for each channel +// (point_data_ip, push_msg_ip, etc.), which constrains which host the +// lidar sends data to. +// * AllowHandle filters packets at the PubHandler level. +// +// Remaining exposure: if the SDK's internal discovery exchange changes the +// lidar's "host" pointer, the only remedy is a longer delay or ensuring the +// two config files use completely non-overlapping port ranges (already done: +// front 56101-56105, back 56111-56115). Increasing back_lidar_delay to 10 s +// or more eliminates nearly all observed failures. +// --------------------------------------------------------------------------- + void LivoxLidarCallback::LidarInfoChangeCallback(const uint32_t handle, const LivoxLidarInfo* info, void* client_data) { if (client_data == nullptr) { - std::cout << "lidar info change callback failed, client data is nullptr" << std::endl; + LIVOX_ERROR("lidar info change callback failed, client data is nullptr"); return; } LdsLidar* lds_lidar = static_cast(client_data); + LIVOX_INFO("[%s] LidarInfoChange event (handle=0x%08x)", + IpNumToString(handle).c_str(), handle); + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); if (lidar_device == nullptr) { - std::cout << "found lidar not defined in the user-defined config, ip: " << IpNumToString(handle) << std::endl; - // add lidar device - uint8_t index = 0; - int8_t ret = lds_lidar->cache_index_.GetFreeIndex(kLivoxLidarType, handle, index); - if (ret != 0) { - std::cout << "failed to add lidar device, lidar ip: " << IpNumToString(handle) << std::endl; - return; + // This lidar is not in this driver instance's user-defined config (it may + // belong to another driver instance on the same host). Do NOT send any + // SDK commands (SetLivoxLidarWorkMode, EnableLivoxLidarImuData, etc.) to + // it: doing so would make the lidar re-route its data stream to this + // process, silently breaking the other driver instance that owns it. + // Log once per foreign IP to avoid flooding the console on every + // periodic SDK re-discovery event. + static std::mutex s_foreign_mutex; + static std::unordered_set s_logged_foreign; + std::lock_guard lock(s_foreign_mutex); + if (s_logged_foreign.insert(handle).second) { + LIVOX_WARN("[%s] ignoring lidar — not in this instance's config" + " (likely owned by another driver process)", + IpNumToString(handle).c_str()); } - LidarDevice *p_lidar = &(lds_lidar->lidars_[index]); - p_lidar->lidar_type = kLivoxLidarType; - } else { + return; + } + + // RACE 2 mitigation: if this lidar is already fully configured and + // streaming, a re-fire of LidarInfoChangeCallback is a periodic SDK + // re-discovery heartbeat. Re-sending all config commands would interrupt + // the active data stream. Only refresh the work mode to keep streaming. + if (lidar_device->connect_state.load(std::memory_order_acquire) == kConnectStateSampling) { + LIVOX_INFO("[%s] LidarInfoChange re-fire (already Sampling)," + " refreshing work mode only", + IpNumToString(handle).c_str()); + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr); + return; + } + + { // set the lidar according to the user-defined config const UserLivoxLidarConfig& config = lidar_device->livox_config; // lock for modify the lidar device set_bits { std::lock_guard lock(lds_lidar->config_mutex_); + uint32_t pending_bits = 0; if (config.pcl_data_type != -1 ) { lidar_device->livox_config.set_bits |= kConfigDataType; + pending_bits |= kConfigDataType; SetLivoxLidarPclDataType(handle, static_cast(config.pcl_data_type), LivoxLidarCallback::SetDataTypeCallback, lds_lidar); - std::cout << "set pcl data type, handle: " << handle << ", data type: " - << static_cast(config.pcl_data_type) << std::endl; + LIVOX_INFO("[%s] requesting pcl data type: %d", + IpNumToString(handle).c_str(), static_cast(config.pcl_data_type)); } if (config.pattern_mode != -1) { lidar_device->livox_config.set_bits |= kConfigScanPattern; + pending_bits |= kConfigScanPattern; SetLivoxLidarScanPattern(handle, static_cast(config.pattern_mode), LivoxLidarCallback::SetPatternModeCallback, lds_lidar); - std::cout << "set scan pattern, handle: " << handle << ", scan pattern: " - << static_cast(config.pattern_mode) << std::endl; + LIVOX_INFO("[%s] requesting scan pattern: %d", + IpNumToString(handle).c_str(), static_cast(config.pattern_mode)); } if (config.blind_spot_set != -1) { lidar_device->livox_config.set_bits |= kConfigBlindSpot; + pending_bits |= kConfigBlindSpot; SetLivoxLidarBlindSpot(handle, config.blind_spot_set, LivoxLidarCallback::SetBlindSpotCallback, lds_lidar); - - std::cout << "set blind spot, handle: " << handle << ", blind spot distance: " - << config.blind_spot_set << std::endl; + LIVOX_INFO("[%s] requesting blind spot: %d", + IpNumToString(handle).c_str(), config.blind_spot_set); } if (config.dual_emit_en != -1) { lidar_device->livox_config.set_bits |= kConfigDualEmit; + pending_bits |= kConfigDualEmit; SetLivoxLidarDualEmit(handle, (config.dual_emit_en == 0 ? false : true), LivoxLidarCallback::SetDualEmitCallback, lds_lidar); - std::cout << "set dual emit mode, handle: " << handle << ", enable dual emit: " - << static_cast(config.dual_emit_en) << std::endl; + LIVOX_INFO("[%s] requesting dual emit: %d", + IpNumToString(handle).c_str(), static_cast(config.dual_emit_en)); + } + if (pending_bits == 0) { + // No config commands needed — mark directly as sampling so data is forwarded. + lidar_device->connect_state.store(kConnectStateSampling, std::memory_order_release); + LIVOX_INFO("[%s] no config commands needed, state -> Sampling", + IpNumToString(handle).c_str()); + } else { + // Count set bits using Brian Kernighan's algorithm (each iteration clears the lowest set bit). + uint32_t pending_count = 0; + for (uint32_t b = pending_bits; b != 0; b &= (b - 1)) { ++pending_count; } + LIVOX_INFO("[%s] waiting for %u config ack(s) before Sampling (pending mask=0x%x)", + IpNumToString(handle).c_str(), pending_count, pending_bits); } } // free lock for set_bits @@ -102,7 +198,7 @@ void LivoxLidarCallback::LidarInfoChangeCallback(const uint32_t handle, LivoxLidarCallback::SetAttitudeCallback, lds_lidar); } - std::cout << "begin to change work mode to 'Normal', handle: " << handle << std::endl; + LIVOX_INFO("[%s] requesting work mode -> Normal", IpNumToString(handle).c_str()); SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr); EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar); return; @@ -113,22 +209,31 @@ void LivoxLidarCallback::WorkModeChangedCallback(livox_status status, LivoxLidarAsyncControlResponse *response, void *client_data) { if (status != kLivoxLidarStatusSuccess) { - std::cout << "failed to change work mode, handle: " << handle << ", try again..."<< std::endl; + // Track per-handle retry count to surface persistent failures. + static std::mutex s_retry_mutex; + static std::unordered_map s_retry_count; + int retries; + { + std::lock_guard lock(s_retry_mutex); + retries = ++s_retry_count[handle]; + } + LIVOX_WARN("[%s] work mode change failed (status=%d), retry #%d", + IpNumToString(handle).c_str(), static_cast(status), retries); std::this_thread::sleep_for(std::chrono::seconds(1)); SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr); return; } - std::cout << "successfully change work mode, handle: " << handle << std::endl; + LIVOX_INFO("[%s] work mode -> Normal: OK", IpNumToString(handle).c_str()); return; } void LivoxLidarCallback::SetDataTypeCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { - LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); if (lidar_device == nullptr) { - std::cout << "failed to set data type since no lidar device found, handle: " - << handle << std::endl; + LIVOX_ERROR("[%s] set data type ack: lidar device not found", + IpNumToString(handle).c_str()); return; } LdsLidar* lds_lidar = static_cast(client_data); @@ -136,21 +241,26 @@ void LivoxLidarCallback::SetDataTypeCallback(livox_status status, uint32_t handl if (status == kLivoxLidarStatusSuccess) { std::lock_guard lock(lds_lidar->config_mutex_); lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDataType)); + LIVOX_INFO("[%s] data type set OK, remaining config bits: %u", + IpNumToString(handle).c_str(), lidar_device->livox_config.set_bits); if (!lidar_device->livox_config.set_bits) { - lidar_device->connect_state = kConnectStateSampling; + lidar_device->connect_state.store(kConnectStateSampling, std::memory_order_release); + LIVOX_INFO("[%s] all config acks received — state -> Sampling, data will be published", + IpNumToString(handle).c_str()); } - std::cout << "successfully set data type, handle: " << handle - << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl; } else if (status == kLivoxLidarStatusTimeout) { const UserLivoxLidarConfig& config = lidar_device->livox_config; SetLivoxLidarPclDataType(handle, static_cast(config.pcl_data_type), LivoxLidarCallback::SetDataTypeCallback, client_data); - std::cout << "set data type timeout, handle: " << handle - << ", try again..." << std::endl; + LIVOX_WARN("[%s] set data type timed out, retrying...", IpNumToString(handle).c_str()); } else { - std::cout << "failed to set data type, handle: " << handle - << ", return code: " << response->ret_code - << ", error key: " << response->error_key << std::endl; + if (response) { + LIVOX_ERROR("[%s] set data type failed: ret_code=%d error_key=%d", + IpNumToString(handle).c_str(), response->ret_code, response->error_key); + } else { + LIVOX_ERROR("[%s] set data type failed: status=%d (no response payload)", + IpNumToString(handle).c_str(), static_cast(status)); + } } return; } @@ -158,10 +268,10 @@ void LivoxLidarCallback::SetDataTypeCallback(livox_status status, uint32_t handl void LivoxLidarCallback::SetPatternModeCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { - LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); if (lidar_device == nullptr) { - std::cout << "failed to set pattern mode since no lidar device found, handle: " - << handle << std::endl; + LIVOX_ERROR("[%s] set pattern mode ack: lidar device not found", + IpNumToString(handle).c_str()); return; } LdsLidar* lds_lidar = static_cast(client_data); @@ -169,21 +279,26 @@ void LivoxLidarCallback::SetPatternModeCallback(livox_status status, uint32_t ha if (status == kLivoxLidarStatusSuccess) { std::lock_guard lock(lds_lidar->config_mutex_); lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigScanPattern)); + LIVOX_INFO("[%s] scan pattern set OK, remaining config bits: %u", + IpNumToString(handle).c_str(), lidar_device->livox_config.set_bits); if (!lidar_device->livox_config.set_bits) { - lidar_device->connect_state = kConnectStateSampling; + lidar_device->connect_state.store(kConnectStateSampling, std::memory_order_release); + LIVOX_INFO("[%s] all config acks received — state -> Sampling, data will be published", + IpNumToString(handle).c_str()); } - std::cout << "successfully set pattern mode, handle: " << handle - << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl; } else if (status == kLivoxLidarStatusTimeout) { const UserLivoxLidarConfig& config = lidar_device->livox_config; SetLivoxLidarScanPattern(handle, static_cast(config.pattern_mode), LivoxLidarCallback::SetPatternModeCallback, client_data); - std::cout << "set pattern mode timeout, handle: " << handle - << ", try again..." << std::endl; + LIVOX_WARN("[%s] set scan pattern timed out, retrying...", IpNumToString(handle).c_str()); } else { - std::cout << "failed to set pattern mode, handle: " << handle - << ", return code: " << response->ret_code - << ", error key: " << response->error_key << std::endl; + if (response) { + LIVOX_ERROR("[%s] set scan pattern failed: ret_code=%d error_key=%d", + IpNumToString(handle).c_str(), response->ret_code, response->error_key); + } else { + LIVOX_ERROR("[%s] set scan pattern failed: status=%d (no response payload)", + IpNumToString(handle).c_str(), static_cast(status)); + } } return; } @@ -191,10 +306,10 @@ void LivoxLidarCallback::SetPatternModeCallback(livox_status status, uint32_t ha void LivoxLidarCallback::SetBlindSpotCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { - LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); if (lidar_device == nullptr) { - std::cout << "failed to set blind spot since no lidar device found, handle: " - << handle << std::endl; + LIVOX_ERROR("[%s] set blind spot ack: lidar device not found", + IpNumToString(handle).c_str()); return; } LdsLidar* lds_lidar = static_cast(client_data); @@ -202,21 +317,26 @@ void LivoxLidarCallback::SetBlindSpotCallback(livox_status status, uint32_t hand if (status == kLivoxLidarStatusSuccess) { std::lock_guard lock(lds_lidar->config_mutex_); lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigBlindSpot)); + LIVOX_INFO("[%s] blind spot set OK, remaining config bits: %u", + IpNumToString(handle).c_str(), lidar_device->livox_config.set_bits); if (!lidar_device->livox_config.set_bits) { - lidar_device->connect_state = kConnectStateSampling; + lidar_device->connect_state.store(kConnectStateSampling, std::memory_order_release); + LIVOX_INFO("[%s] all config acks received — state -> Sampling, data will be published", + IpNumToString(handle).c_str()); } - std::cout << "successfully set blind spot, handle: " << handle - << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl; } else if (status == kLivoxLidarStatusTimeout) { const UserLivoxLidarConfig& config = lidar_device->livox_config; SetLivoxLidarBlindSpot(handle, config.blind_spot_set, LivoxLidarCallback::SetBlindSpotCallback, client_data); - std::cout << "set blind spot timeout, handle: " << handle - << ", try again..." << std::endl; + LIVOX_WARN("[%s] set blind spot timed out, retrying...", IpNumToString(handle).c_str()); } else { - std::cout << "failed to set blind spot, handle: " << handle - << ", return code: " << response->ret_code - << ", error key: " << response->error_key << std::endl; + if (response) { + LIVOX_ERROR("[%s] set blind spot failed: ret_code=%d error_key=%d", + IpNumToString(handle).c_str(), response->ret_code, response->error_key); + } else { + LIVOX_ERROR("[%s] set blind spot failed: status=%d (no response payload)", + IpNumToString(handle).c_str(), static_cast(status)); + } } return; } @@ -224,10 +344,10 @@ void LivoxLidarCallback::SetBlindSpotCallback(livox_status status, uint32_t hand void LivoxLidarCallback::SetDualEmitCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { - LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); if (lidar_device == nullptr) { - std::cout << "failed to set dual emit mode since no lidar device found, handle: " - << handle << std::endl; + LIVOX_ERROR("[%s] set dual emit ack: lidar device not found", + IpNumToString(handle).c_str()); return; } @@ -235,21 +355,26 @@ void LivoxLidarCallback::SetDualEmitCallback(livox_status status, uint32_t handl if (status == kLivoxLidarStatusSuccess) { std::lock_guard lock(lds_lidar->config_mutex_); lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDualEmit)); + LIVOX_INFO("[%s] dual emit set OK, remaining config bits: %u", + IpNumToString(handle).c_str(), lidar_device->livox_config.set_bits); if (!lidar_device->livox_config.set_bits) { - lidar_device->connect_state = kConnectStateSampling; + lidar_device->connect_state.store(kConnectStateSampling, std::memory_order_release); + LIVOX_INFO("[%s] all config acks received — state -> Sampling, data will be published", + IpNumToString(handle).c_str()); } - std::cout << "successfully set dual emit mode, handle: " << handle - << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl; } else if (status == kLivoxLidarStatusTimeout) { const UserLivoxLidarConfig& config = lidar_device->livox_config; SetLivoxLidarDualEmit(handle, config.dual_emit_en, LivoxLidarCallback::SetDualEmitCallback, client_data); - std::cout << "set dual emit mode timeout, handle: " << handle - << ", try again..." << std::endl; + LIVOX_WARN("[%s] set dual emit timed out, retrying...", IpNumToString(handle).c_str()); } else { - std::cout << "failed to set dual emit mode, handle: " << handle - << ", return code: " << response->ret_code - << ", error key: " << response->error_key << std::endl; + if (response) { + LIVOX_ERROR("[%s] set dual emit failed: ret_code=%d error_key=%d", + IpNumToString(handle).c_str(), response->ret_code, response->error_key); + } else { + LIVOX_ERROR("[%s] set dual emit failed: status=%d (no response payload)", + IpNumToString(handle).c_str(), static_cast(status)); + } } return; } @@ -257,19 +382,18 @@ void LivoxLidarCallback::SetDualEmitCallback(livox_status status, uint32_t handl void LivoxLidarCallback::SetAttitudeCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { - LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); if (lidar_device == nullptr) { - std::cout << "failed to set dual emit mode since no lidar device found, handle: " - << handle << std::endl; + LIVOX_ERROR("[%s] set attitude ack: lidar device not found", + IpNumToString(handle).c_str()); return; } LdsLidar* lds_lidar = static_cast(client_data); if (status == kLivoxLidarStatusSuccess) { - std::cout << "successfully set lidar attitude, ip: " << IpNumToString(handle) << std::endl; + LIVOX_INFO("[%s] install attitude set OK", IpNumToString(handle).c_str()); } else if (status == kLivoxLidarStatusTimeout) { - std::cout << "set lidar attitude timeout, ip: " << IpNumToString(handle) - << ", try again..." << std::endl; + LIVOX_WARN("[%s] set attitude timed out, retrying...", IpNumToString(handle).c_str()); const UserLivoxLidarConfig& config = lidar_device->livox_config; LivoxLidarInstallAttitude attitude { config.extrinsic_param.roll, @@ -282,41 +406,40 @@ void LivoxLidarCallback::SetAttitudeCallback(livox_status status, uint32_t handl SetLivoxLidarInstallAttitude(config.handle, &attitude, LivoxLidarCallback::SetAttitudeCallback, lds_lidar); } else { - std::cout << "failed to set lidar attitude, ip: " << IpNumToString(handle) << std::endl; + LIVOX_ERROR("[%s] set attitude failed (status=%d)", + IpNumToString(handle).c_str(), static_cast(status)); } } void LivoxLidarCallback::EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { - LidarDevice* lidar_device = GetLidarDevice(handle, client_data); + LidarDevice* lidar_device = GetLidarDevice(handle, client_data); if (lidar_device == nullptr) { - std::cout << "failed to set pattern mode since no lidar device found, handle: " - << handle << std::endl; + LIVOX_ERROR("[%s] enable IMU ack: lidar device not found", IpNumToString(handle).c_str()); return; } LdsLidar* lds_lidar = static_cast(client_data); if (response == nullptr) { - std::cout << "failed to get response since no lidar IMU sensor found, handle: " - << handle << std::endl; + LIVOX_WARN("[%s] enable IMU: no response received", IpNumToString(handle).c_str()); return; } if (status == kLivoxLidarStatusSuccess) { - std::cout << "successfully enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl; + LIVOX_INFO("[%s] IMU data enabled OK", IpNumToString(handle).c_str()); } else if (status == kLivoxLidarStatusTimeout) { - std::cout << "enable Livox Lidar imu timeout, ip: " << IpNumToString(handle) - << ", try again..." << std::endl; + LIVOX_WARN("[%s] enable IMU timed out, retrying...", IpNumToString(handle).c_str()); EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar); } else { - std::cout << "failed to enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl; + LIVOX_ERROR("[%s] enable IMU failed (status=%d)", + IpNumToString(handle).c_str(), static_cast(status)); } } LidarDevice* LivoxLidarCallback::GetLidarDevice(const uint32_t handle, void* client_data) { if (client_data == nullptr) { - std::cout << "failed to get lidar device, client data is nullptr" << std::endl; + LIVOX_ERROR("GetLidarDevice: client data is nullptr"); return nullptr; } @@ -331,3 +454,4 @@ LidarDevice* LivoxLidarCallback::GetLidarDevice(const uint32_t handle, void* cli } } // namespace livox_ros + diff --git a/src/comm/comm.h b/src/comm/comm.h index 492b39e1..38c6f31d 100644 --- a/src/comm/comm.h +++ b/src/comm/comm.h @@ -25,6 +25,7 @@ #ifndef LIVOX_ROS_DRIVER2_COMM_H_ #define LIVOX_ROS_DRIVER2_COMM_H_ +#include #include #include #include @@ -278,7 +279,15 @@ typedef struct { // uint8_t handle : 4; // handle for LivoxLidarType::kIndustryLidarType // }; uint8_t data_src; /**< From raw lidar or livox file. */ - volatile LidarConnectState connect_state; + // connect_state is written by SDK callback threads (under config_mutex_) and + // read by polling threads without a lock. std::atomic ensures the update is + // immediately visible across threads without relying on volatile, which only + // prevents compiler optimisation and not CPU reordering. + std::atomic connect_state; + // Set to true the first time a point cloud or IMU packet arrives from this + // lidar. Used by the reconnect watchdog to detect when the lidar is in + // Sampling state but no data is flowing (Race 3 symptom). + std::atomic data_received{false}; // DeviceInfo info; LidarDataQueue data; diff --git a/src/comm/pub_handler.cpp b/src/comm/pub_handler.cpp index 07d4d4ff..203ec8ad 100644 --- a/src/comm/pub_handler.cpp +++ b/src/comm/pub_handler.cpp @@ -26,8 +26,10 @@ #include #include -#include #include +#include + +#include "../include/livox_log.h" namespace livox_ros { @@ -89,10 +91,20 @@ void PubHandler::ClearAllLidarsExtrinsicParams() { lidar_extrinsics_.clear(); } +void PubHandler::AllowHandle(uint32_t handle) { + std::lock_guard lock(allowed_handles_mutex_); + allowed_handles_.insert(handle); +} + void PubHandler::SetPointCloudsCallback(PointCloudsCallback cb, void* client_data) { pub_client_data_ = client_data; points_callback_ = cb; lidar_listen_id_ = LivoxLidarAddPointCloudObserver(OnLivoxLidarPointCloudCallback, this); + if (lidar_listen_id_ == 0) { + LIVOX_ERROR("LivoxLidarAddPointCloudObserver failed — no point cloud data will be received"); + } else { + LIVOX_INFO("point cloud observer registered (listen_id=%u)", lidar_listen_id_); + } } void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type, @@ -102,6 +114,48 @@ void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t d return; } + // Log the first packet received from each handle to confirm data is flowing. + // This MUST be before the AllowHandle check so we can detect Race 3: if the + // front driver's SDK is receiving the back lidar's data (and silently dropping + // it), this log will appear in the front driver's output, revealing that the + // back lidar's data stream is being routed to the wrong process. + { + static std::unordered_set s_first_packet_seen; + static std::mutex s_first_packet_mutex; + std::lock_guard lock(s_first_packet_mutex); + if (s_first_packet_seen.insert(handle).second) { + LIVOX_INFO("[%s] first packet received (dev_type=%u, data_type=%u)", + IpNumToString(handle).c_str(), dev_type, data->data_type); + } + } + + // Drop packets from lidars not explicitly configured for this driver instance. + // This prevents cross-talk when two driver nodes run on the same host and the + // SDK observer fires for all discovered lidars regardless of which node owns them. + { + std::lock_guard lock(self->allowed_handles_mutex_); + if (!self->allowed_handles_.empty() && + self->allowed_handles_.find(handle) == self->allowed_handles_.end()) { + // Throttled warning: if this fires in the FRONT driver for the back + // lidar's IP (or vice versa), it confirms Race 3 is occurring — the + // SDK routed the wrong lidar's data stream to this process. + static std::unordered_map s_drop_warn_last; + static std::mutex s_drop_warn_mutex; + { + std::lock_guard wlock(s_drop_warn_mutex); + auto now = std::chrono::steady_clock::now(); + auto& last = s_drop_warn_last[handle]; + if (now - last > std::chrono::seconds(10)) { + last = now; + LIVOX_WARN("[%s] dropping packet — handle not in this driver's" + " allowed set (Race 3: SDK data mis-routing?)", + IpNumToString(handle).c_str()); + } + } + return; // foreign lidar — drop + } + } + if (data->time_type != kTimestampTypeNoSync) { is_timestamp_sync_.store(true); } else { @@ -309,8 +363,9 @@ void LidarPubHandler::PointCloudProcess(RawPacket & pkt) { } else { static bool flag = false; if (!flag) { - std::cout << "error, unsupported protocol type: " << static_cast(pkt.lidar_type) << std::endl; - flag = true; + flag = true; + LIVOX_ERROR("PointCloudProcess: unsupported protocol type %d", + static_cast(pkt.lidar_type)); } } } @@ -327,8 +382,8 @@ void LidarPubHandler::LivoxLidarPointCloudProcess(RawPacket & pkt) { ProcessSphericalPoint(pkt); break; default: - std::cout << "unknown data type: " << static_cast(pkt.data_type) - << " !!" << std::endl; + LIVOX_ERROR("LivoxLidarPointCloudProcess: unknown data type %d", + static_cast(pkt.data_type)); break; } } diff --git a/src/comm/pub_handler.h b/src/comm/pub_handler.h index 9c519a09..b6587bc5 100644 --- a/src/comm/pub_handler.h +++ b/src/comm/pub_handler.h @@ -34,6 +34,7 @@ #include #include // std::mutex #include +#include #include "livox_lidar_def.h" #include "livox_lidar_api.h" @@ -90,6 +91,9 @@ class PubHandler { void AddLidarsExtParam(LidarExtParameter& extrinsic_params); void ClearAllLidarsExtrinsicParams(); void SetImuDataCallback(ImuDataCallback cb, void* client_data); + // Register a lidar handle (IP as uint32) that this driver instance owns. + // Only packets from registered handles are processed; all others are dropped. + void AllowHandle(uint32_t handle); private: //thread to process raw data @@ -129,6 +133,9 @@ class PubHandler { std::map lidar_extrinsics_; static std::atomic is_timestamp_sync_; uint16_t lidar_listen_id_ = 0; + // Allowlist of lidar handles (= IP as uint32) this instance is configured for. + std::unordered_set allowed_handles_; + std::mutex allowed_handles_mutex_; }; PubHandler &pub_handler(); diff --git a/src/driver_node.h b/src/driver_node.h index aaf4f81c..6379c9bd 100644 --- a/src/driver_node.h +++ b/src/driver_node.h @@ -70,6 +70,10 @@ class DriverNode final : public rclcpp::Node { std::shared_ptr imudata_poll_thread_; std::shared_future future_; std::promise exit_signal_; + /** One-shot timer: fires if no lidar connects within connect_timeout_s. */ + rclcpp::TimerBase::SharedPtr watchdog_timer_; + /** Periodic timer: re-asserts work mode for Sampling lidars with no data yet (Race 3 recovery). */ + rclcpp::TimerBase::SharedPtr reconnect_timer_; }; #endif diff --git a/src/include/livox_log.h b/src/include/livox_log.h new file mode 100644 index 00000000..3592acac --- /dev/null +++ b/src/include/livox_log.h @@ -0,0 +1,52 @@ +// MIT License — see project root for full license text. +// +// livox_log.h +// ----------- +// Lightweight, node-free logging macros for internal driver code that runs +// outside of a ROS node context (callbacks, data-processing threads, etc.). +// +// Usage (printf-style format string): +// LIVOX_INFO("lidar %s ready", ip_str.c_str()); +// LIVOX_WARN("retry #%d for %s", n, ip_str.c_str()); +// +// ROS2 : routes to RCLCPP_* with a named "livox_driver" logger. +// ROS1 : routes to the standard ROS_* macros. +// Other: falls back to fprintf(stderr/stdout). + +#ifndef LIVOX_LOG_H_ +#define LIVOX_LOG_H_ + +#ifdef BUILDING_ROS2 + +#include +// Retrieve a cached named logger so the overhead is negligible. +namespace livox_ros { namespace detail { +inline rclcpp::Logger get_logger() { + return rclcpp::get_logger("livox_driver"); +} +}} // namespace livox_ros::detail + +#define LIVOX_DEBUG(...) RCLCPP_DEBUG(livox_ros::detail::get_logger(), __VA_ARGS__) +#define LIVOX_INFO(...) RCLCPP_INFO (livox_ros::detail::get_logger(), __VA_ARGS__) +#define LIVOX_WARN(...) RCLCPP_WARN (livox_ros::detail::get_logger(), __VA_ARGS__) +#define LIVOX_ERROR(...) RCLCPP_ERROR(livox_ros::detail::get_logger(), __VA_ARGS__) + +#elif defined BUILDING_ROS1 + +#include +#define LIVOX_DEBUG(...) ROS_DEBUG(__VA_ARGS__) +#define LIVOX_INFO(...) ROS_INFO(__VA_ARGS__) +#define LIVOX_WARN(...) ROS_WARN(__VA_ARGS__) +#define LIVOX_ERROR(...) ROS_ERROR(__VA_ARGS__) + +#else // non-ROS build (tools, unit tests) + +#include +#define LIVOX_DEBUG(...) do {} while (0) +#define LIVOX_INFO(...) do { fprintf(stdout, __VA_ARGS__); fprintf(stdout, "\n"); } while (0) +#define LIVOX_WARN(...) do { fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n"); } while (0) +#define LIVOX_ERROR(...) do { fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n"); } while (0) + +#endif // BUILDING_ROS2 / BUILDING_ROS1 + +#endif // LIVOX_LOG_H_ diff --git a/src/lddc.cpp b/src/lddc.cpp index 06195df4..4bd03ef9 100644 --- a/src/lddc.cpp +++ b/src/lddc.cpp @@ -26,13 +26,14 @@ #include "comm/ldq.h" #include "comm/comm.h" +#include #include -#include -#include #include #include +#include #include "include/ros_headers.h" +#include "include/livox_log.h" #include "driver_node.h" #include "lds_lidar.h" @@ -103,7 +104,7 @@ Lddc::~Lddc() { } } #endif - std::cout << "lddc destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl; + LIVOX_INFO("Lddc destroyed"); } int Lddc::RegisterLds(Lds *lds) { @@ -117,33 +118,46 @@ int Lddc::RegisterLds(Lds *lds) { void Lddc::DistributePointCloudData(void) { if (!lds_) { - std::cout << "lds is not registered" << std::endl; + LIVOX_ERROR("DistributePointCloudData: LDS not registered"); return; } if (lds_->IsRequestExit()) { - std::cout << "DistributePointCloudData is RequestExit" << std::endl; + LIVOX_DEBUG("DistributePointCloudData: exit requested"); return; } - + lds_->pcd_semaphore_.Wait(); for (uint32_t i = 0; i < lds_->lidar_count_; i++) { uint32_t lidar_id = i; LidarDevice *lidar = &lds_->lidars_[lidar_id]; - LidarDataQueue *p_queue = &lidar->data; - if ((kConnectStateSampling != lidar->connect_state) || (p_queue == nullptr)) { + if (kConnectStateSampling != lidar->connect_state.load(std::memory_order_acquire)) { + // Emit a throttled warning so the user can see which lidar is stuck + // in the init state and not yet publishing data. + if (lidar->handle != 0) { + static std::unordered_map s_last_warn; + auto now = std::chrono::steady_clock::now(); + auto& last = s_last_warn[lidar->handle]; + if (now - last > std::chrono::seconds(10)) { + last = now; + LIVOX_WARN("[%s] not yet in Sampling state (connect_state=%d)," + " point cloud will not be published", + IpNumToString(lidar->handle).c_str(), + static_cast(lidar->connect_state.load(std::memory_order_acquire))); + } + } continue; } - PollingLidarPointCloudData(lidar_id, lidar); + PollingLidarPointCloudData(lidar_id, lidar); } } void Lddc::DistributeImuData(void) { if (!lds_) { - std::cout << "lds is not registered" << std::endl; + LIVOX_ERROR("DistributeImuData: LDS not registered"); return; } if (lds_->IsRequestExit()) { - std::cout << "DistributeImuData is RequestExit" << std::endl; + LIVOX_DEBUG("DistributeImuData: exit requested"); return; } @@ -151,8 +165,7 @@ void Lddc::DistributeImuData(void) { for (uint32_t i = 0; i < lds_->lidar_count_; i++) { uint32_t lidar_id = i; LidarDevice *lidar = &lds_->lidars_[lidar_id]; - LidarImuDataQueue *p_queue = &lidar->imu_data; - if ((kConnectStateSampling != lidar->connect_state) || (p_queue == nullptr)) { + if (kConnectStateSampling != lidar->connect_state.load(std::memory_order_acquire)) { continue; } PollingLidarImuData(lidar_id, lidar); @@ -203,7 +216,7 @@ void Lddc::PublishPointcloud2(LidarDataQueue *queue, uint8_t index) { StoragePacket pkg; QueuePop(queue, &pkg); if (pkg.points.empty()) { - printf("Publish point cloud2 failed, the pkg points is empty.\n"); + LIVOX_WARN("PublishPointCloud2: pkg points is empty, skipping"); continue; } @@ -219,7 +232,7 @@ void Lddc::PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index) { StoragePacket pkg; QueuePop(queue, &pkg); if (pkg.points.empty()) { - printf("Publish custom point cloud failed, the pkg points is empty.\n"); + LIVOX_WARN("PublishCustomPointCloud: pkg points is empty, skipping"); continue; } @@ -235,18 +248,17 @@ void Lddc::PublishPclMsg(LidarDataQueue *queue, uint8_t index) { #ifdef BUILDING_ROS2 static bool first_log = true; if (first_log) { - std::cout << "error: message type 'pcl::PointCloud' is NOT supported in ROS2, " - << "please modify the 'xfer_format' field in the launch file" - << std::endl; + first_log = false; + LIVOX_ERROR("message type 'pcl::PointCloud' is NOT supported in ROS2," + " please set 'xfer_format' to a supported type in the launch file"); } - first_log = false; return; #endif while(!QueueIsEmpty(queue)) { StoragePacket pkg; QueuePop(queue, &pkg); if (pkg.points.empty()) { - printf("Publish point cloud failed, the pkg points is empty.\n"); + LIVOX_WARN("PublishPclMsg: pkg points is empty, skipping"); continue; } @@ -376,7 +388,7 @@ void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t if (lds_->lidars_[index].lidar_type == kLivoxLidarType) { livox_msg.lidar_id = lds_->lidars_[index].handle; } else { - printf("Init custom msg lidar id failed, the index:%u.\n", index); + LIVOX_ERROR("InitCustomMsg: unknown lidar type for index %u", index); livox_msg.lidar_id = 0; } } @@ -427,9 +439,7 @@ void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& tim } cloud.header.stamp = timestamp / 1000.0; // to pcl ros time stamp #elif defined BUILDING_ROS2 - std::cout << "warning: pcl::PointCloud is not supported in ROS2, " - << "please check code logic" - << std::endl; + LIVOX_WARN("InitPclMsg: pcl::PointCloud is not supported in ROS2 — check code logic"); #endif return; } @@ -452,9 +462,7 @@ void Lddc::FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg) { pcl_msg.points.push_back(std::move(point)); } #elif defined BUILDING_ROS2 - std::cout << "warning: pcl::PointCloud is not supported in ROS2, " - << "please check code logic" - << std::endl; + LIVOX_WARN("FillPointsToPclMsg: pcl::PointCloud is not supported in ROS2 — check code logic"); #endif return; } @@ -470,9 +478,7 @@ void Lddc::PublishPclData(const uint8_t index, const uint64_t timestamp, const P } } #elif defined BUILDING_ROS2 - std::cout << "warning: pcl::PointCloud is not supported in ROS2, " - << "please check code logic" - << std::endl; + LIVOX_WARN("PublishPclData: pcl::PointCloud is not supported in ROS2 — check code logic"); #endif return; } diff --git a/src/lds.cpp b/src/lds.cpp index ca984628..0e8effa3 100644 --- a/src/lds.cpp +++ b/src/lds.cpp @@ -31,6 +31,7 @@ #include "lds.h" #include "comm/ldq.h" +#include "include/livox_log.h" namespace livox_ros { @@ -50,7 +51,7 @@ Lds::Lds(const double publish_freq, const uint8_t data_src) Lds::~Lds() { lidar_count_ = 0; ResetLds(0); - printf("lds destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"); + LIVOX_INFO("Lds destroyed"); } void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) { @@ -59,7 +60,8 @@ void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) { lidar->imu_data.Clear(); lidar->data_src = data_src; - lidar->connect_state = kConnectStateOff; + lidar->connect_state.store(kConnectStateOff, std::memory_order_release); + lidar->data_received.store(false, std::memory_order_release); } void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) { @@ -101,15 +103,14 @@ void Lds::StorageImuData(ImuData* imu_data) { if (imu_data->lidar_type == kLivoxLidarType) { device_num = imu_data->handle; } else { - printf("Storage imu data failed, unknown lidar type:%u.\n", imu_data->lidar_type); + LIVOX_ERROR("StorageImuData: unknown lidar type %u", imu_data->lidar_type); return; } uint8_t index = 0; int ret = cache_index_.GetIndex(imu_data->lidar_type, device_num, index); if (ret != 0) { - printf("Storage point data failed, can not get index, lidar type:%u, device_num:%u.\n", imu_data->lidar_type, device_num); - return; + return; // unknown handle — already filtered upstream by PubHandler allowlist } LidarDevice *p_lidar = &lidars_[index]; @@ -135,11 +136,12 @@ void Lds::StorageLvxPointData(PointFrame* frame) { uint8_t index = 0; int8_t ret = cache_index_.LvxGetIndex(lidar_point.lidar_type, lidar_point.handle, index); if (ret != 0) { - printf("Storage lvx point data failed, lidar type:%u, device num:%u.\n", lidar_point.lidar_type, lidar_point.handle); + LIVOX_ERROR("StorageLvxPointData: unknown lidar type %u handle %u", + lidar_point.lidar_type, lidar_point.handle); continue; } - lidars_[index].connect_state = kConnectStateSampling; + lidars_[index].connect_state.store(kConnectStateSampling, std::memory_order_release); PushLidarData(&lidar_point, index, base_time); } @@ -160,8 +162,7 @@ void Lds::StoragePointData(PointFrame* frame) { uint8_t index = 0; int8_t ret = cache_index_.GetIndex(lidar_point.lidar_type, lidar_point.handle, index); if (ret != 0) { - printf("Storage point data failed, lidar type:%u, handle:%u.\n", lidar_point.lidar_type, lidar_point.handle); - continue; + continue; // unknown handle — already filtered upstream by PubHandler allowlist } PushLidarData(&lidar_point, index, base_time); } @@ -178,11 +179,12 @@ void Lds::PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint if (nullptr == queue->storage_packet) { uint32_t queue_size = CalculatePacketQueueSize(publish_freq_); InitQueue(queue, queue_size); - printf("Lidar[%u] storage queue size: %u\n", index, queue_size); + LIVOX_INFO("lidar[%u] storage queue initialised, size: %u", index, queue_size); } if (!QueueIsFull(queue)) { QueuePushAny(queue, (uint8_t *)lidar_data, base_time); + p_lidar->data_received.store(true, std::memory_order_release); if (!QueueIsEmpty(queue)) { if (pcd_semaphore_.GetCount() <= 0) { pcd_semaphore_.Signal(); diff --git a/src/lds_lidar.cpp b/src/lds_lidar.cpp index 6c739b08..cbe90b3a 100644 --- a/src/lds_lidar.cpp +++ b/src/lds_lidar.cpp @@ -50,7 +50,8 @@ #include "call_back/lidar_common_callback.h" #include "call_back/livox_lidar_callback.h" -using namespace std; +#include "include/livox_log.h" + namespace livox_ros { @@ -78,7 +79,7 @@ void LdsLidar::ResetLdsLidar(void) { ResetLds(kSourceRawLidar); } bool LdsLidar::InitLdsLidar(const std::string& path_name) { if (is_initialized_) { - printf("Lds is already inited!\n"); + LIVOX_WARN("LdsLidar is already initialized, ignoring duplicate init call"); return false; } @@ -102,7 +103,7 @@ bool LdsLidar::InitLidars() { if (!ParseSummaryConfig()) { return false; } - std::cout << "config lidar type: " << static_cast(lidar_summary_info_.lidar_type) << std::endl; + LIVOX_INFO("config lidar type: %d", static_cast(lidar_summary_info_.lidar_type)); if (lidar_summary_info_.lidar_type & kLivoxLidarType) { if (!InitLivoxLidar()) { @@ -135,21 +136,23 @@ bool LdsLidar::InitLivoxLidar() { LivoxLidarConfigParser parser(path_); std::vector user_configs; if (!parser.Parse(user_configs)) { - std::cout << "failed to parse user-defined config" << std::endl; + LIVOX_ERROR("failed to parse user-defined lidar config from: %s", path_.c_str()); } // SDK initialization if (!LivoxLidarSdkInit(path_.c_str())) { - std::cout << "Failed to init livox lidar sdk." << std::endl; + LIVOX_ERROR("failed to init Livox Lidar SDK with config: %s", path_.c_str()); return false; } + LIVOX_INFO("Livox Lidar SDK initialised, config: %s", path_.c_str()); // fill in lidar devices for (auto& config : user_configs) { uint8_t index = 0; int8_t ret = g_lds_ldiar->cache_index_.GetFreeIndex(kLivoxLidarType, config.handle, index); if (ret != 0) { - std::cout << "failed to get free index, lidar ip: " << IpNumToString(config.handle) << std::endl; + LIVOX_ERROR("failed to get free cache index for lidar IP %s", + IpNumToString(config.handle).c_str()); continue; } LidarDevice *p_lidar = &(g_lds_ldiar->lidars_[index]); @@ -157,6 +160,11 @@ bool LdsLidar::InitLivoxLidar() { p_lidar->livox_config = config; p_lidar->handle = config.handle; + // Register this handle so PubHandler drops packets from foreign lidars. + pub_handler().AllowHandle(config.handle); + LIVOX_INFO("registered lidar IP %s (handle=0x%08x, cache index=%u)", + IpNumToString(config.handle).c_str(), config.handle, index); + LidarExtParameter lidar_param; lidar_param.handle = config.handle; lidar_param.lidar_type = kLivoxLidarType; @@ -197,13 +205,13 @@ bool LdsLidar::LivoxLidarStart() { int LdsLidar::DeInitLdsLidar(void) { if (!is_initialized_) { - printf("LiDAR data source is not exit"); + LIVOX_WARN("DeInitLdsLidar called but LiDAR data source was not initialised"); return -1; } if (lidar_summary_info_.lidar_type & kLivoxLidarType) { LivoxLidarSdkUninit(); - printf("Livox Lidar SDK Deinit completely!\n"); + LIVOX_INFO("Livox Lidar SDK uninitialised"); } return 0; diff --git a/src/livox_ros_driver2.cpp b/src/livox_ros_driver2.cpp index 6dda05f8..19994f49 100644 --- a/src/livox_ros_driver2.cpp +++ b/src/livox_ros_driver2.cpp @@ -30,6 +30,7 @@ #include "include/livox_ros_driver2.h" #include "include/ros_headers.h" +#include "include/livox_log.h" #include "driver_node.h" #include "lddc.h" #include "lds_lidar.h" @@ -137,6 +138,7 @@ DriverNode::DriverNode(const rclcpp::NodeOptions & node_options) this->declare_parameter("user_config_path", "path_default"); this->declare_parameter("cmdline_input_bd_code", "000000000000001"); this->declare_parameter("lvx_file_path", "/home/livox/livox_test.lvx"); + this->declare_parameter("connect_timeout_s", 10.0); this->get_parameter("xfer_format", xfer_format); this->get_parameter("multi_topic", multi_topic); @@ -174,6 +176,100 @@ DriverNode::DriverNode(const rclcpp::NodeOptions & node_options) if ((read_lidar->InitLdsLidar(user_config_path))) { DRIVER_INFO(*this, "Init lds lidar success!"); + + // Watchdog: if no lidar contacts us within connect_timeout_s, the + // hardware is unreachable. Shut down so the node can be respawned. + double connect_timeout_s; + this->get_parameter("connect_timeout_s", connect_timeout_s); + Lds* lds = lddc_ptr_->lds_; + watchdog_timer_ = this->create_wall_timer( + std::chrono::duration(connect_timeout_s), + [this, lds, connect_timeout_s]() { + watchdog_timer_->cancel(); // one-shot + bool any_connected = false; + for (uint32_t i = 0; i < kMaxSourceLidar; i++) { + if (lds->lidars_[i].lidar_type != 0 && + lds->lidars_[i].connect_state.load(std::memory_order_acquire) + != kConnectStateOff) { + any_connected = true; + break; + } + } + if (!any_connected) { + RCLCPP_FATAL(this->get_logger(), + "No lidar connected within %.1f s — hardware unreachable or " + "not responding on the network. Shutting down for respawn.", + connect_timeout_s); + // Escalating shutdown in a detached thread so the timer callback + // returns immediately: SIGTERM first (gives rclcpp a chance to + // clean up), then SIGKILL after 5 s to guarantee the process exits. + std::thread([]() { + raise(SIGTERM); + std::this_thread::sleep_for(std::chrono::seconds(5)); + raise(SIGKILL); + }).detach(); + } + }); + + // Race-3 recovery timer: fires every 3 s and re-sends SetLivoxLidarWorkMode + // + EnableLivoxLidarImuData for any lidar that has reached kConnectStateSampling + // but has not yet delivered any data. + // + // Root cause of Race 3: when the second driver instance starts, the Livox + // SDK it initialises discovers ALL lidars on the network and sends each one + // a "host registration" packet (host IP + data ports from the config file). + // If the first driver's host_net_info entry is not correctly scoped to only + // its own lidar IP, this registration overwrites the second driver's, causing + // the lidar to send its data stream to the first driver's ports where it is + // silently dropped by the AllowHandle filter. + // + // Mitigation: re-sending SetLivoxLidarWorkMode from THIS driver's cmd_data_port + // causes the lidar to update its "active host" to the sender of this command. + // Subsequent data will then be routed to this driver's data ports. The timer + // cancels itself once all configured lidars have confirmed data flow. + reconnect_timer_ = this->create_wall_timer( + std::chrono::seconds(3), + [this, lds]() { + bool all_flowing = true; + for (uint32_t i = 0; i < lds->lidar_count_; i++) { + LidarDevice& dev = lds->lidars_[i]; + if (dev.lidar_type == 0) continue; // slot not in use + if (dev.connect_state.load(std::memory_order_acquire) + != kConnectStateSampling) continue; // not ready yet + if (dev.data_received.load(std::memory_order_acquire)) continue; // flowing ✓ + // Lidar is in Sampling state but no data has arrived yet. + // Re-assert ownership by re-sending work mode from this driver. + RCLCPP_WARN(this->get_logger(), + "[%s] in Sampling state but no data received — Race 3 suspected." + " Re-asserting work mode to reclaim data stream.", + IpNumToString(dev.handle).c_str()); + SetLivoxLidarWorkMode(dev.handle, kLivoxLidarNormal, + [](livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse*, void* /*ctx*/) { + if (status == kLivoxLidarStatusSuccess) { + LIVOX_INFO("[%s] reconnect work-mode refresh OK", + IpNumToString(handle).c_str()); + } else { + LIVOX_WARN("[%s] reconnect work-mode refresh failed (status=%d)", + IpNumToString(handle).c_str(), static_cast(status)); + } + }, nullptr); + // Re-send IMU enable so the lidar re-registers its IMU data destination. + EnableLivoxLidarImuData(dev.handle, + [](livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse*, void*) { + if (status != kLivoxLidarStatusSuccess) { + LIVOX_WARN("[%s] reconnect IMU re-enable failed (status=%d)", + IpNumToString(handle).c_str(), static_cast(status)); + } + }, nullptr); + all_flowing = false; + } + if (all_flowing) { + reconnect_timer_->cancel(); + DRIVER_INFO(*this, "All lidars are flowing; reconnect timer cancelled."); + } + }); } else { DRIVER_ERROR(*this, "Init lds lidar fail!"); } diff --git a/src/tools/livox_scan.cpp b/src/tools/livox_scan.cpp new file mode 100644 index 00000000..d8736163 --- /dev/null +++ b/src/tools/livox_scan.cpp @@ -0,0 +1,300 @@ +/** + * livox_scan -- Discover all Livox LiDAR devices visible on the network + * and report their settings. + * + * Usage: + * livox_scan [host_ip [timeout_seconds]] + * + * Examples: + * # Scan using auto-detected host NIC for 10 seconds (default): + * livox_scan + * + * # Scan from a specific NIC: + * livox_scan 192.168.1.10 + * + * # Scan from a specific NIC with a 5-second timeout: + * livox_scan 192.168.1.10 5 + * + * Build: + * See CMakeLists.txt + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// -------------------------------------------------------------------------- +// Per-device information collected during the scan +// -------------------------------------------------------------------------- + +struct DeviceEntry { + uint32_t handle; + std::string lidar_ip; + std::string sn; + uint8_t dev_type; +}; + +// -------------------------------------------------------------------------- +// Global state +// -------------------------------------------------------------------------- + +struct ScanState { + std::mutex mtx; + std::vector devices; +}; + +static ScanState g_scan; + +// -------------------------------------------------------------------------- +// Helper: map numeric device type to a human-readable string +// -------------------------------------------------------------------------- +static const char* DevTypeName(uint8_t dev_type) { + switch (dev_type) { + case kLivoxLidarTypeHub: return "Hub"; + case kLivoxLidarTypeMid40: return "Mid-40"; + case kLivoxLidarTypeTele: return "Tele-15"; + case kLivoxLidarTypeHorizon: return "Horizon"; + case kLivoxLidarTypeMid70: return "Mid-70"; + case kLivoxLidarTypeAvia: return "Avia"; + case kLivoxLidarTypeMid360: return "Mid-360"; + case kLivoxLidarTypeIndustrialHAP: return "Industrial HAP"; + case kLivoxLidarTypeHAP: return "HAP"; + case kLivoxLidarTypePA: return "PA"; + default: return "Unknown"; + } +} + +// -------------------------------------------------------------------------- +// Callback: called by the SDK whenever a lidar is discovered or its info +// changes (multiple calls for the same device are de-duplicated) +// -------------------------------------------------------------------------- +static void OnLidarInfoChange(const uint32_t handle, + const LivoxLidarInfo* info, + void* /*client_data*/) { + if (!info) return; + + std::lock_guard lock(g_scan.mtx); + + // De-duplicate by handle + for (const auto& d : g_scan.devices) { + if (d.handle == handle) return; + } + + DeviceEntry entry; + entry.handle = handle; + entry.lidar_ip = info->lidar_ip; + entry.sn = info->sn; + entry.dev_type = info->dev_type; + + std::cout << "[found] " << entry.lidar_ip + << " type=" << DevTypeName(entry.dev_type) + << " sn=" << entry.sn + << "\n"; + + g_scan.devices.push_back(std::move(entry)); +} + +// -------------------------------------------------------------------------- +// Write a minimal Livox SDK config to a temp file so that the SDK has a +// valid device-info map structure before handling discovery responses. +// Passing nullptr for the config causes GetFirmwareType() to dereference +// an uninitialised map entry → SIGSEGV when any lidar replies. +// -------------------------------------------------------------------------- +static std::string WriteTempConfig(const std::string& host_ip) { + // Use /tmp with a unique-ish name + std::string path = "/tmp/livox_scan_config_" + std::to_string(getpid()) + ".json"; + + // Choose a non-empty host IP for the config; fall back to a harmless value + // if the caller didn't provide one (the SDK will still auto-pick the NIC). + const std::string ip = host_ip.empty() ? "0.0.0.0" : host_ip; + + std::ofstream f(path); + if (!f.is_open()) return ""; + + // lidar_type 8 = MID360 (kLivoxLidarTypeMid360). + // lidar_configs is intentionally empty — discovery finds whatever is present. + f << "{\n" + << " \"lidar_summary_info\": { \"lidar_type\": 8 },\n" + << " \"MID360\": {\n" + << " \"lidar_net_info\": {\n" + << " \"cmd_data_port\": 56100,\n" + << " \"push_msg_port\": 56200,\n" + << " \"point_data_port\": 56300,\n" + << " \"imu_data_port\": 56400,\n" + << " \"log_data_port\": 56500\n" + << " },\n" + << " \"host_net_info\": {\n" + << " \"cmd_data_ip\": \"" << ip << "\",\n" + << " \"cmd_data_port\": 56101,\n" + << " \"push_msg_ip\": \"" << ip << "\",\n" + << " \"push_msg_port\": 56201,\n" + << " \"point_data_ip\": \"" << ip << "\",\n" + << " \"point_data_port\": 56301,\n" + << " \"imu_data_ip\": \"" << ip << "\",\n" + << " \"imu_data_port\": 56401,\n" + << " \"log_data_ip\": \"\",\n" + << " \"log_data_port\": 56501\n" + << " }\n" + << " },\n" + << " \"lidar_configs\": []\n" + << "}\n"; + + return f.good() ? path : ""; +} + +// -------------------------------------------------------------------------- +// Usage +// -------------------------------------------------------------------------- +static void PrintUsage(const char* prog, std::ostream& out = std::cout) { + out + << "Usage: " << prog << " [OPTIONS] [host_ip [timeout_seconds]]\n\n" + << "Options:\n" + << " -h, --help Show this help message and exit\n\n" + << "Arguments:\n" + << " host_ip [optional] IP of the host NIC facing the LiDARs\n" + << " (omit to let the SDK auto-detect)\n" + << " timeout_seconds [optional] How long to scan in seconds (default: 10)\n\n" + << "Examples:\n" + << " " << prog << "\n" + << " " << prog << " --help\n" + << " " << prog << " 192.168.1.10\n" + << " " << prog << " 192.168.1.10 5\n"; +} + +// -------------------------------------------------------------------------- +// main +// -------------------------------------------------------------------------- +int main(int argc, char** argv) { + // Handle --help / -h before any other processing + for (int i = 1; i < argc; ++i) { + if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + PrintUsage(argv[0]); + return 0; + } + } + + if (argc > 3) { + PrintUsage(argv[0], std::cerr); + return 1; + } + + std::string host_ip = (argc >= 2) ? argv[1] : ""; + int timeout_secs = 10; + + if (argc == 3) { + try { + timeout_secs = std::stoi(argv[2]); + if (timeout_secs <= 0) throw std::invalid_argument("non-positive"); + } catch (...) { + std::cerr << "[err] Invalid timeout value: " << argv[2] << "\n"; + PrintUsage(argv[0], std::cerr); + return 1; + } + } + + std::cout << "=== Livox LiDAR scanner ===\n"; + if (!host_ip.empty()) { + std::cout << " Host NIC : " << host_ip << "\n"; + } else { + std::cout << " Host NIC : (auto-detect)\n"; + } + std::cout << " Timeout : " << timeout_secs << " s\n\n"; + + // Silence the SDK's own console output so our messages are uncluttered + DisableLivoxSdkConsoleLogger(); + + // Register the discovery callback BEFORE initialising the SDK + SetLivoxLidarInfoChangeCallback(OnLidarInfoChange, nullptr); + + // Write a minimal config so that the SDK's internal DeviceInfo map is + // properly initialised before discovery responses arrive. Passing nullptr + // here causes GetFirmwareType() to dereference an uninitialised entry + // → SIGSEGV (reproduced on liblivox_lidar_sdk_shared.so). + std::string tmp_cfg = WriteTempConfig(host_ip); + if (tmp_cfg.empty()) { + std::cerr << "[warn] Could not write temporary config; " + "SDK may crash on discovery responses.\n"; + } + + const char* cfg_path = tmp_cfg.empty() ? nullptr : tmp_cfg.c_str(); + + if (!LivoxLidarSdkInit(cfg_path, host_ip.c_str())) { + std::cerr << "[err] LivoxLidarSdkInit() failed. " + "Check that the host IP / NIC is correct.\n"; + if (!tmp_cfg.empty()) std::remove(tmp_cfg.c_str()); + return 1; + } + + std::cout << "[info] Scanning for " << timeout_secs << " second(s) ...\n\n"; + + std::this_thread::sleep_for(std::chrono::seconds(timeout_secs)); + + LivoxLidarSdkUninit(); + + // Remove the temporary config file written to avoid the SDK SIGSEGV + if (!tmp_cfg.empty()) std::remove(tmp_cfg.c_str()); + + // ----------------------------------------------------------------------- + // Print summary table + // ----------------------------------------------------------------------- + const auto& devs = g_scan.devices; + + std::cout << "\n"; + + if (devs.empty()) { + std::cout << "[result] No LiDAR devices found.\n" + << " Make sure the device is powered, the host NIC is in " + "the same subnet,\n" + << " and that no firewall is blocking UDP traffic.\n"; + return 1; + } + + std::cout << "[result] Found " << devs.size() + << " device(s):\n\n"; + + // Column widths + const int w_ip = 16; + const int w_type = 16; + const int w_sn = 18; + + auto hline = [&]() { + std::cout << "+" << std::string(w_ip + 2, '-') + << "+" << std::string(w_type + 2, '-') + << "+" << std::string(w_sn + 2, '-') + << "+\n"; + }; + + hline(); + std::cout << "| " << std::left << std::setw(w_ip) << "IP Address" + << " | " << std::setw(w_type) << "Device Type" + << " | " << std::setw(w_sn) << "Serial Number" + << " |\n"; + hline(); + + for (const auto& d : devs) { + std::cout << "| " << std::left << std::setw(w_ip) << d.lidar_ip + << " | " << std::setw(w_type) << DevTypeName(d.dev_type) + << " | " << std::setw(w_sn) << d.sn + << " |\n"; + } + + hline(); + std::cout << "\n"; + + return 0; +} diff --git a/src/tools/livox_set_ip.cpp b/src/tools/livox_set_ip.cpp new file mode 100644 index 00000000..478ed9d1 --- /dev/null +++ b/src/tools/livox_set_ip.cpp @@ -0,0 +1,277 @@ +/** + * livox_set_ip -- Set the static IP of a Livox MID-360 (or any Livox-SDK2 device) + * + * Usage: + * livox_set_ip [host_ip] + * + * Examples: + * # Connect from host 192.168.1.10, change lidar from factory IP to static IP: + * livox_set_ip 192.168.1.100 192.168.1.167 255.255.255.0 192.168.1.1 192.168.1.10 + * + * # If host IP is not specified, SDK will auto-detect it: + * livox_set_ip 192.168.1.167 192.168.1.184 255.255.255.0 192.168.1.1 + * + * Build: + * See CMakeLists.txt + * + * IMPORTANT: Power-cycle the lidar after running this tool for the new IP to take effect. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// -------------------------------------------------------------------------- +// Global state shared between callbacks and main thread +// -------------------------------------------------------------------------- + +struct AppState { + std::string target_lidar_ip; // The lidar we want to configure (current IP) + std::string new_ip; + std::string new_netmask; + std::string new_gateway; + + std::atomic lidar_found{false}; + std::atomic ip_set_done{false}; + std::atomic ip_set_ok{false}; + + std::mutex mtx; + std::condition_variable cv; +}; + +static AppState g_state; + +// -------------------------------------------------------------------------- +// Helper: convert uint32_t handle (packed IPv4) back to dotted string +// -------------------------------------------------------------------------- +static std::string HandleToIp(uint32_t handle) { + // SDK encodes the lidar IP as a big-endian uint32 handle + char buf[20]; + snprintf(buf, sizeof(buf), "%u.%u.%u.%u", + (handle >> 24) & 0xFF, + (handle >> 16) & 0xFF, + (handle >> 8) & 0xFF, + (handle ) & 0xFF); + return std::string(buf); +} + +// -------------------------------------------------------------------------- +// Callback: called by the SDK whenever a lidar is discovered or its info +// changes (connects, state change, etc.) +// -------------------------------------------------------------------------- +static void OnLidarInfoChange(const uint32_t handle, + const LivoxLidarInfo* info, + void* /*client_data*/) { + if (!info) return; + + std::string discovered_ip(info->lidar_ip); + + std::cout << "[info] Discovered lidar ip=" << discovered_ip + << " sn=" << info->sn + << " dev_type=" << static_cast(info->dev_type) + << "\n"; + + // Is this the lidar we are looking for? + if (discovered_ip != g_state.target_lidar_ip) { + return; + } + + if (g_state.lidar_found.exchange(true)) { + return; // already being handled + } + + std::cout << "[info] Target lidar found, sending IP-change command ...\n"; + + // Build the new IP config + LivoxLidarIpInfo ip_cfg{}; + strncpy(ip_cfg.ip_addr, g_state.new_ip.c_str(), sizeof(ip_cfg.ip_addr) - 1); + strncpy(ip_cfg.net_mask, g_state.new_netmask.c_str(), sizeof(ip_cfg.net_mask) - 1); + strncpy(ip_cfg.gw_addr, g_state.new_gateway.c_str(), sizeof(ip_cfg.gw_addr) - 1); + + // Async callback for the IP-set command response + auto ip_set_cb = [](livox_status status, + uint32_t /*handle*/, + LivoxLidarAsyncControlResponse* resp, + void* /*client_data*/) { + if (status == kLivoxLidarStatusSuccess && resp && resp->ret_code == 0) { + std::cout << "[ok] IP change accepted by lidar. " + "Power-cycle the device to apply the new IP.\n"; + g_state.ip_set_ok = true; + } else { + std::cout << "[err] IP change failed." + << " sdk_status=" << status; + if (resp) { + std::cout << " ret_code=" << static_cast(resp->ret_code) + << " error_key=" << resp->error_key; + } + std::cout << "\n"; + g_state.ip_set_ok = false; + } + g_state.ip_set_done = true; + g_state.cv.notify_all(); + }; + + livox_status rc = SetLivoxLidarIp(handle, &ip_cfg, ip_set_cb, nullptr); + if (rc != kLivoxLidarStatusSuccess) { + std::cerr << "[err] SetLivoxLidarIp() returned error code " << rc << "\n"; + g_state.ip_set_ok = false; + g_state.ip_set_done = true; + g_state.cv.notify_all(); + } +} + +// -------------------------------------------------------------------------- +// Write a minimal Livox SDK config to a temp file so that the SDK has a +// valid device-info map structure before handling discovery responses. +// Passing nullptr for the config causes GetFirmwareType() to dereference +// an uninitialised map entry → SIGSEGV when any lidar replies. +// -------------------------------------------------------------------------- +static std::string WriteTempConfig(const std::string& host_ip) { + std::string path = "/tmp/livox_set_ip_config_" + std::to_string(getpid()) + ".json"; + const std::string ip = host_ip.empty() ? "0.0.0.0" : host_ip; + + std::ofstream f(path); + if (!f.is_open()) return ""; + + f << "{\n" + << " \"lidar_summary_info\": { \"lidar_type\": 8 },\n" + << " \"MID360\": {\n" + << " \"lidar_net_info\": {\n" + << " \"cmd_data_port\": 56100,\n" + << " \"push_msg_port\": 56200,\n" + << " \"point_data_port\": 56300,\n" + << " \"imu_data_port\": 56400,\n" + << " \"log_data_port\": 56500\n" + << " },\n" + << " \"host_net_info\": {\n" + << " \"cmd_data_ip\": \"" << ip << "\",\n" + << " \"cmd_data_port\": 56101,\n" + << " \"push_msg_ip\": \"" << ip << "\",\n" + << " \"push_msg_port\": 56201,\n" + << " \"point_data_ip\": \"" << ip << "\",\n" + << " \"point_data_port\": 56301,\n" + << " \"imu_data_ip\": \"" << ip << "\",\n" + << " \"imu_data_port\": 56401,\n" + << " \"log_data_ip\": \"\",\n" + << " \"log_data_port\": 56501\n" + << " }\n" + << " },\n" + << " \"lidar_configs\": []\n" + << "}\n"; + + return f.good() ? path : ""; +} + +// -------------------------------------------------------------------------- +// Usage +// -------------------------------------------------------------------------- +static void PrintUsage(const char* prog) { + std::cerr + << "Usage: " << prog + << " [host_ip]\n\n" + << " current_lidar_ip Current IP of the lidar to configure\n" + << " new_ip New static IP to assign to the lidar\n" + << " netmask Subnet mask (e.g. 255.255.255.0)\n" + << " gateway Gateway address (e.g. 192.168.1.1)\n" + << " host_ip [optional] IP of the NIC connected to the lidar\n" + << " (omit to let the SDK auto-detect)\n\n" + << "Example:\n" + << " " << prog + << " 192.168.1.111 192.168.1.167 255.255.255.0 192.168.1.1 192.168.1.10\n\n" + << "IMPORTANT: Power-cycle the lidar after this tool completes.\n"; +} + +// -------------------------------------------------------------------------- +// main +// -------------------------------------------------------------------------- +int main(int argc, char** argv) { + if (argc < 5 || argc > 6) { + PrintUsage(argv[0]); + return 1; + } + + g_state.target_lidar_ip = argv[1]; + g_state.new_ip = argv[2]; + g_state.new_netmask = argv[3]; + g_state.new_gateway = argv[4]; + std::string host_ip = (argc == 6) ? argv[5] : ""; + + std::cout << "=== Livox IP configuration tool ===\n" + << " Target lidar (current IP) : " << g_state.target_lidar_ip << "\n" + << " New IP : " << g_state.new_ip << "\n" + << " Netmask : " << g_state.new_netmask << "\n" + << " Gateway : " << g_state.new_gateway << "\n"; + if (!host_ip.empty()) { + std::cout << " Host NIC IP : " << host_ip << "\n"; + } + std::cout << "\n"; + + // Silence the SDK's verbose console output + DisableLivoxSdkConsoleLogger(); + + // Register the device-discovery callback BEFORE initialising the SDK + SetLivoxLidarInfoChangeCallback(OnLidarInfoChange, nullptr); + + // Write a minimal config so the SDK's internal DeviceInfo map is properly + // initialised before discovery responses arrive. Passing nullptr here + // causes GetFirmwareType() to dereference an uninitialised entry → SIGSEGV. + std::string tmp_cfg = WriteTempConfig(host_ip); + if (tmp_cfg.empty()) { + std::cerr << "[warn] Could not write temporary config; " + "SDK may crash on discovery responses.\n"; + } + + const char* cfg_path = tmp_cfg.empty() ? nullptr : tmp_cfg.c_str(); + + if (!LivoxLidarSdkInit(cfg_path, host_ip.c_str())) { + std::cerr << "[err] LivoxLidarSdkInit() failed. " + "Check that the host IP / NIC is correct.\n"; + if (!tmp_cfg.empty()) std::remove(tmp_cfg.c_str()); + return 1; + } + + std::cout << "[info] SDK initialised, scanning for lidar at " + << g_state.target_lidar_ip << " ...\n"; + + // Wait up to 15 seconds for the IP-set command to complete + constexpr int kTimeoutSeconds = 15; + std::unique_lock lock(g_state.mtx); + bool completed = g_state.cv.wait_for( + lock, + std::chrono::seconds(kTimeoutSeconds), + [] { return g_state.ip_set_done.load(); }); + + LivoxLidarSdkUninit(); + + // Remove the temporary config file written to avoid the SDK SIGSEGV + if (!tmp_cfg.empty()) std::remove(tmp_cfg.c_str()); + + if (!completed) { + std::cerr << "[err] Timed out after " << kTimeoutSeconds << "s. " + "Lidar not found or not responding.\n" + << " Make sure the lidar is powered, reachable at " + << g_state.target_lidar_ip + << ",\n and that your host NIC is in the same subnet.\n"; + return 1; + } + + if (!g_state.ip_set_ok) { + std::cerr << "[err] IP configuration FAILED.\n"; + return 1; + } + + std::cout << "\n[ok] Done. New IP: " << g_state.new_ip << "\n" + << " Please POWER-CYCLE the lidar now.\n"; + return 0; +} \ No newline at end of file From e662c7bbe8dd0b1706e04cfe8cb932d539d44644 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sun, 22 Mar 2026 17:21:25 +0000 Subject: [PATCH 3/7] feat: add livox_reboot CLI tool to soft-reboot or reset Livox LiDAR devices Co-authored-by: marc-hanheide <1153084+marc-hanheide@users.noreply.github.com> Agent-Logs-Url: https://github.com/jabasai/livox_ros_driver2/sessions/72f95b72-8a82-4ee4-80cf-8dde957f7536 --- CMakeLists.txt | 12 ++ README.md | 35 +++ src/tools/livox_reboot.cpp | 422 +++++++++++++++++++++++++++++++++++++ 3 files changed, 469 insertions(+) create mode 100644 src/tools/livox_reboot.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 02a9dda2..1e084c0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -362,6 +362,18 @@ else(ROS_EDITION STREQUAL "ROS2") DESTINATION lib/${PROJECT_NAME} ) + # livox_reboot: standalone command-line tool to reboot or reset Livox devices + add_executable(livox_reboot src/tools/livox_reboot.cpp) + target_include_directories(livox_reboot PRIVATE + ${LIVOX_LIDAR_SDK_INCLUDE_DIR} + ) + target_link_libraries(livox_reboot + ${LIVOX_LIDAR_SDK_LIBRARY} + ) + install(TARGETS livox_reboot + DESTINATION lib/${PROJECT_NAME} + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/README.md b/README.md index 55ab5d32..75862297 100644 --- a/README.md +++ b/README.md @@ -705,6 +705,41 @@ ros2 run livox_ros_driver2 livox_scan 192.168.1.10 5 +------------------+------------------+--------------------+ ``` +### 5.3 livox_reboot — Reboot or reset LiDAR devices + +`livox_reboot` sends a soft **reboot** (firmware-triggered restart) or **reset** (configuration/state reset) command to one or more Livox LiDARs over the network. It is useful for recovering from a hung state or applying settings that require a restart. + +> ⚠️ This is a **soft reboot over the network** — it does not cut physical power. Allow ~30 seconds for the device to come back online. + +**Usage:** + +```shell +ros2 run livox_ros_driver2 livox_reboot [ip2 ...] [--reset] [--host-ip ] +ros2 run livox_ros_driver2 livox_reboot --all [--reset] [--host-ip ] +``` + +| Option | Description | +| --- | --- | +| `--reset` | Send a Reset command (config/state reset) instead of a full Reboot | +| `--host-ip ` | *(optional)* IP of the host NIC; auto-detected if omitted | +| `--all` | Target all discovered devices — use with caution! | + +**Examples:** + +```shell +# Reboot a single device +ros2 run livox_ros_driver2 livox_reboot 192.168.1.167 + +# Reboot both lidars simultaneously +ros2 run livox_ros_driver2 livox_reboot 192.168.1.167 192.168.1.184 + +# Reset (not reboot) all discovered devices +ros2 run livox_ros_driver2 livox_reboot --all --reset + +# Reboot with an explicit host NIC address +ros2 run livox_ros_driver2 livox_reboot 192.168.1.167 --host-ip 192.168.1.10 +``` + --- ## 6. Supported LiDAR list diff --git a/src/tools/livox_reboot.cpp b/src/tools/livox_reboot.cpp new file mode 100644 index 00000000..79e441d0 --- /dev/null +++ b/src/tools/livox_reboot.cpp @@ -0,0 +1,422 @@ +/** + * livox_reboot -- Soft-reboot or reset one or more Livox LiDAR devices + * + * Usage: + * livox_reboot [ip2 ...] [--reset] [--host-ip ] + * livox_reboot --all [--reset] [--host-ip ] + * + * Options: + * --reset Send a Reset command instead of a Reboot. + * Reset clears device configuration/state; Reboot is a + * full firmware restart equivalent to a power-cycle. + * --host-ip IP of the host NIC connected to the lidar(s). + * Omit to let the SDK auto-detect. + * --all Target every device discovered on the network. + * Use with caution! + * + * Examples: + * # Reboot a single device + * livox_reboot 192.168.1.167 + * + * # Reboot two devices simultaneously + * livox_reboot 192.168.1.167 192.168.1.184 + * + * # Reset all discovered devices + * livox_reboot --all --reset + * + * # Reboot with an explicit host NIC address + * livox_reboot 192.168.1.167 --host-ip 192.168.1.10 + * + * Build: + * See CMakeLists.txt + * + * NOTE: This issues a soft reboot (firmware-triggered restart) over the + * network — it does NOT cut and restore physical power. After a + * reboot the device typically comes back online within ~30 seconds. + * If you need a true power-cycle, use external hardware (smart PDU / + * relay board). + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// --------------------------------------------------------------------------- +// Per-device job state +// --------------------------------------------------------------------------- + +struct DeviceJob { + std::string ip; + bool triggered{false}; // reboot/reset command has been sent + bool completed{false}; // callback received + bool success{false}; // command reported success +}; + +// --------------------------------------------------------------------------- +// Global application state +// --------------------------------------------------------------------------- + +struct AppState { + std::set target_ips; // empty when --all is used + bool reboot_all{false}; + bool use_reset{false}; + + std::vector jobs; + std::mutex mtx; + std::condition_variable cv; +}; + +static AppState g_state; + +// --------------------------------------------------------------------------- +// Helper: convert uint32_t handle (packed IPv4) to dotted-decimal string +// --------------------------------------------------------------------------- +static std::string HandleToIp(uint32_t handle) { + char buf[20]; + snprintf(buf, sizeof(buf), "%u.%u.%u.%u", + (handle >> 24) & 0xFF, + (handle >> 16) & 0xFF, + (handle >> 8) & 0xFF, + (handle ) & 0xFF); + return std::string(buf); +} + +// --------------------------------------------------------------------------- +// Callbacks for Reboot and Reset +// --------------------------------------------------------------------------- + +static void OnRebootResponse(livox_status status, uint32_t handle, + LivoxLidarRebootResponse* response, + void* /*client_data*/) { + std::string ip = HandleToIp(handle); + std::lock_guard lock(g_state.mtx); + + for (auto& job : g_state.jobs) { + if (job.ip == ip && !job.completed) { + job.completed = true; + if (status == kLivoxLidarStatusSuccess && response && + response->ret_code == 0) { + job.success = true; + std::cout << "[ok] " << ip << " reboot command accepted.\n"; + } else { + job.success = false; + int ret = response ? static_cast(response->ret_code) : -1; + std::cerr << "[err] " << ip << " reboot failed" + << " status=" << status + << " ret_code=" << ret << "\n"; + } + break; + } + } + g_state.cv.notify_all(); +} + +static void OnResetResponse(livox_status status, uint32_t handle, + LivoxLidarResetResponse* response, + void* /*client_data*/) { + std::string ip = HandleToIp(handle); + std::lock_guard lock(g_state.mtx); + + for (auto& job : g_state.jobs) { + if (job.ip == ip && !job.completed) { + job.completed = true; + if (status == kLivoxLidarStatusSuccess && response && + response->ret_code == 0) { + job.success = true; + std::cout << "[ok] " << ip << " reset command accepted.\n"; + } else { + job.success = false; + int ret = response ? static_cast(response->ret_code) : -1; + std::cerr << "[err] " << ip << " reset failed" + << " status=" << status + << " ret_code=" << ret << "\n"; + } + break; + } + } + g_state.cv.notify_all(); +} + +// --------------------------------------------------------------------------- +// Discovery callback: fires when a lidar is found (or its state changes). +// If the discovered device is in our target list (or --all is set) and we +// haven't yet sent it a command, do so now. +// --------------------------------------------------------------------------- +static void OnLidarInfoChange(const uint32_t handle, + const LivoxLidarInfo* info, + void* /*client_data*/) { + if (!info) return; + + std::string discovered_ip(info->lidar_ip); + std::cout << "[info] Discovered lidar ip=" << discovered_ip << "\n"; + + std::lock_guard lock(g_state.mtx); + + // Decide whether this device is a target + bool is_target = g_state.reboot_all || + (g_state.target_ips.count(discovered_ip) > 0); + if (!is_target) return; + + // Find (or create) the job for this IP + DeviceJob* job = nullptr; + for (auto& j : g_state.jobs) { + if (j.ip == discovered_ip) { job = &j; break; } + } + if (!job) { + // --all path: add a new job on first discovery + g_state.jobs.push_back(DeviceJob{discovered_ip}); + job = &g_state.jobs.back(); + } + + if (job->triggered) return; // already sent — don't double-send + job->triggered = true; + + // Send the command (outside the lock is safer, but the SDK callbacks are + // async so holding the mutex for the send is fine here) + livox_status rc; + if (g_state.use_reset) { + std::cout << "[info] Sending Reset to " << discovered_ip << " ...\n"; + rc = LivoxLidarRequestReset(handle, OnResetResponse, nullptr); + } else { + std::cout << "[info] Sending Reboot to " << discovered_ip << " ...\n"; + rc = LivoxLidarRequestReboot(handle, OnRebootResponse, nullptr); + } + + if (rc != kLivoxLidarStatusSuccess) { + std::cerr << "[err] Command send failed for " << discovered_ip + << " rc=" << rc << "\n"; + job->completed = true; + job->success = false; + g_state.cv.notify_all(); + } +} + +// --------------------------------------------------------------------------- +// Write a minimal SDK config to a temp file to prevent a SIGSEGV that occurs +// when the SDK tries to look up an uninitialised device-info map entry. +// --------------------------------------------------------------------------- +static std::string WriteTempConfig(const std::string& host_ip) { + std::string path = "/tmp/livox_reboot_config_" + + std::to_string(getpid()) + ".json"; + const std::string ip = host_ip.empty() ? "0.0.0.0" : host_ip; + + std::ofstream f(path); + if (!f.is_open()) return ""; + + f << "{\n" + << " \"lidar_summary_info\": { \"lidar_type\": 8 },\n" + << " \"MID360\": {\n" + << " \"lidar_net_info\": {\n" + << " \"cmd_data_port\": 56100,\n" + << " \"push_msg_port\": 56200,\n" + << " \"point_data_port\": 56300,\n" + << " \"imu_data_port\": 56400,\n" + << " \"log_data_port\": 56500\n" + << " },\n" + << " \"host_net_info\": {\n" + << " \"cmd_data_ip\": \"" << ip << "\",\n" + << " \"cmd_data_port\": 56101,\n" + << " \"push_msg_ip\": \"" << ip << "\",\n" + << " \"push_msg_port\": 56201,\n" + << " \"point_data_ip\": \"" << ip << "\",\n" + << " \"point_data_port\": 56301,\n" + << " \"imu_data_ip\": \"" << ip << "\",\n" + << " \"imu_data_port\": 56401,\n" + << " \"log_data_ip\": \"\",\n" + << " \"log_data_port\": 56501\n" + << " }\n" + << " },\n" + << " \"lidar_configs\": []\n" + << "}\n"; + + return f.good() ? path : ""; +} + +// --------------------------------------------------------------------------- +// Usage +// --------------------------------------------------------------------------- +static void PrintUsage(const char* prog) { + std::cerr + << "Usage:\n" + << " " << prog << " [ip2 ...] [--reset] [--host-ip ]\n" + << " " << prog << " --all [--reset] [--host-ip ]\n\n" + << "Options:\n" + << " --reset Send Reset instead of Reboot\n" + << " --host-ip Host NIC IP (auto-detected if omitted)\n" + << " --all Target all discovered devices\n\n" + << "Examples:\n" + << " " << prog << " 192.168.1.167\n" + << " " << prog << " 192.168.1.167 192.168.1.184\n" + << " " << prog << " --all --reset\n" + << " " << prog << " 192.168.1.167 --host-ip 192.168.1.10\n"; +} + +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- +int main(int argc, char** argv) { + if (argc < 2) { + PrintUsage(argv[0]); + return 1; + } + + std::string host_ip; + bool show_help = false; + + // Parse arguments + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg == "--help" || arg == "-h") { + show_help = true; + } else if (arg == "--all") { + g_state.reboot_all = true; + } else if (arg == "--reset") { + g_state.use_reset = true; + } else if (arg == "--host-ip") { + if (i + 1 >= argc) { + std::cerr << "[err] --host-ip requires an argument.\n"; + return 1; + } + host_ip = argv[++i]; + } else if (arg.rfind("--", 0) == 0) { + std::cerr << "[err] Unknown option: " << arg << "\n"; + PrintUsage(argv[0]); + return 1; + } else { + // Treat as a lidar IP address + g_state.target_ips.insert(arg); + g_state.jobs.push_back(DeviceJob{arg}); + } + } + + if (show_help) { + PrintUsage(argv[0]); + return 0; + } + + if (!g_state.reboot_all && g_state.target_ips.empty()) { + std::cerr << "[err] No target IP addresses specified.\n"; + PrintUsage(argv[0]); + return 1; + } + + const char* cmd = g_state.use_reset ? "Reset" : "Reboot"; + + std::cout << "=== Livox LiDAR " << cmd << " tool ===\n"; + if (g_state.reboot_all) { + std::cout << " Targets : all discovered devices\n"; + } else { + for (const auto& ip : g_state.target_ips) { + std::cout << " Target : " << ip << "\n"; + } + } + if (!host_ip.empty()) { + std::cout << " Host NIC : " << host_ip << "\n"; + } + std::cout << " Command : " << cmd << "\n\n"; + + // Silence the SDK's own console output + DisableLivoxSdkConsoleLogger(); + + // Register the discovery callback BEFORE initialising the SDK + SetLivoxLidarInfoChangeCallback(OnLidarInfoChange, nullptr); + + // Write a minimal config so that the SDK's internal DeviceInfo map is + // properly initialised before discovery responses arrive (prevents SIGSEGV). + std::string tmp_cfg = WriteTempConfig(host_ip); + if (tmp_cfg.empty()) { + std::cerr << "[warn] Could not write temporary config; " + "SDK may crash on discovery responses.\n"; + } + + const char* cfg_path = tmp_cfg.empty() ? nullptr : tmp_cfg.c_str(); + const char* sdk_host = host_ip.empty() ? nullptr : host_ip.c_str(); + + if (!LivoxLidarSdkInit(cfg_path, sdk_host)) { + std::cerr << "[err] LivoxLidarSdkInit() failed. " + "Check that the host IP / NIC is correct.\n"; + if (!tmp_cfg.empty()) std::remove(tmp_cfg.c_str()); + return 1; + } + + // Wait for all jobs to complete (or timeout). + // For specific IPs: exit as soon as every job has a response. + // For --all: allow a discovery grace period so the SDK can find all + // devices before we start checking for completion, then exit early + // once every discovered device has received its response. + constexpr int kTimeoutSeconds = 15; + + // --all grace period: sleep without holding any lock so callbacks can + // update g_state.jobs freely during device discovery. + if (g_state.reboot_all) { + std::this_thread::sleep_for(std::chrono::seconds(5)); + } + + auto all_done = [&]() -> bool { + if (g_state.jobs.empty()) return false; // nothing found yet + for (const auto& job : g_state.jobs) { + if (!job.completed) return false; + } + return true; + }; + + // Remaining timeout after the optional discovery grace period. + int remaining_s = g_state.reboot_all ? (kTimeoutSeconds - 5) : kTimeoutSeconds; + + std::unique_lock lock(g_state.mtx); + g_state.cv.wait_for(lock, std::chrono::seconds(remaining_s), all_done); + + LivoxLidarSdkUninit(); + + if (!tmp_cfg.empty()) std::remove(tmp_cfg.c_str()); + + // Print summary + std::cout << "\n=== Summary ===\n"; + int ok_count = 0; + int fail_count = 0; + + for (const auto& job : g_state.jobs) { + if (!job.triggered) { + std::cerr << "[err] " << job.ip + << " — device not found (not reachable or wrong IP?)\n"; + ++fail_count; + } else if (!job.completed) { + std::cerr << "[err] " << job.ip << " — timed out waiting for response.\n"; + ++fail_count; + } else if (!job.success) { + ++fail_count; // already printed by callback + } else { + ++ok_count; + } + } + + if (g_state.reboot_all && g_state.jobs.empty()) { + std::cerr << "[err] No devices discovered within " << kTimeoutSeconds + << " s.\n"; + return 1; + } + + std::cout << " " << ok_count << " device(s) successfully commanded.\n"; + if (fail_count > 0) { + std::cout << " " << fail_count << " device(s) failed.\n"; + } + if (!g_state.use_reset) { + std::cout << "\nNote: after a reboot, allow ~30 s for the device(s) to " + "come back online.\n"; + } + + return fail_count > 0 ? 1 : 0; +} From 7438fcb237990c8d5273dba7bde54ec0fe516ce1 Mon Sep 17 00:00:00 2001 From: ibrahim Date: Sun, 22 Mar 2026 17:50:30 +0000 Subject: [PATCH 4/7] feat: enhance device configuration reporting and add firmware query callbacks --- src/tools/livox_scan.cpp | 191 +++++++++++++++++++++++++++++++++++---- 1 file changed, 175 insertions(+), 16 deletions(-) diff --git a/src/tools/livox_scan.cpp b/src/tools/livox_scan.cpp index d8736163..535e6a74 100644 --- a/src/tools/livox_scan.cpp +++ b/src/tools/livox_scan.cpp @@ -42,11 +42,26 @@ // Per-device information collected during the scan // -------------------------------------------------------------------------- +struct ConfigInfo { + std::string firmware_version; + std::string work_mode; + int32_t pcl_data_type; + int32_t scan_pattern; + uint32_t blind_spot; + bool dual_emit; + bool imu_enabled; + bool config_queried; + + ConfigInfo() : pcl_data_type(-1), scan_pattern(-1), blind_spot(0), + dual_emit(false), imu_enabled(false), config_queried(false) {} +}; + struct DeviceEntry { uint32_t handle; std::string lidar_ip; std::string sn; uint8_t dev_type; + ConfigInfo config; }; // -------------------------------------------------------------------------- @@ -79,6 +94,101 @@ static const char* DevTypeName(uint8_t dev_type) { } } +// -------------------------------------------------------------------------- +// Helper: convert point data type enum to string +// -------------------------------------------------------------------------- +static const char* DataTypeName(int32_t type) { + switch (type) { + case 0x01: return "Cartesian High"; + case 0x02: return "Cartesian Low"; + case 0x03: return "Spherical"; + default: return "Unknown"; + } +} + +// -------------------------------------------------------------------------- +// Helper: convert scan pattern enum to string +// -------------------------------------------------------------------------- +static const char* ScanPatternName(int32_t pattern) { + switch (pattern) { + case 0x00: return "Non-Repetitive"; + case 0x01: return "Repetitive"; + case 0x02: return "Repetitive Low FPS"; + default: return "Unknown"; + } +} + +// -------------------------------------------------------------------------- +// Helper: find device entry by handle +// -------------------------------------------------------------------------- +static DeviceEntry* FindDeviceByHandle(uint32_t handle) { + std::lock_guard lock(g_scan.mtx); + for (auto& d : g_scan.devices) { + if (d.handle == handle) return &d; + } + return nullptr; +} + +// -------------------------------------------------------------------------- +// Callback: firmware version query response +// -------------------------------------------------------------------------- +static void OnFirmwareVersionQuery(livox_status status, + uint32_t handle, + LivoxLidarDiagInternalInfoResponse* response, + void* /*client_data*/) { + DeviceEntry* dev = FindDeviceByHandle(handle); + if (!dev) return; + + if (status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) { + // Parse firmware version from response data + // The format is typically a param_num followed by key-value pairs + if (response->param_num > 0 && response->data[0]) { + // Simple extraction - firmware version is typically a string + dev->config.firmware_version = std::string(reinterpret_cast(response->data)); + } + } + dev->config.config_queried = true; +} + +// -------------------------------------------------------------------------- +// Callback: internal info query response +// -------------------------------------------------------------------------- +static void OnInternalInfoQuery(livox_status status, + uint32_t handle, + LivoxLidarDiagInternalInfoResponse* response, + void* /*client_data*/) { + DeviceEntry* dev = FindDeviceByHandle(handle); + if (!dev) return; + + if (status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) { + std::cout << "[info] Received internal config for " << dev->lidar_ip + << " (param_num=" << response->param_num << ")\n"; + + // Parse key-value parameters from the response + // The data format is: [key(2B)][length(2B)][value(length B)] repeated + uint8_t* ptr = response->data; + size_t offset = 0; + + for (int i = 0; i < response->param_num && offset < 1024; ++i) { + if (offset + 4 > 1024) break; + + uint16_t key = *reinterpret_cast(ptr + offset); + offset += 2; + uint16_t length = *reinterpret_cast(ptr + offset); + offset += 2; + + if (offset + length > 1024) break; + + // Parse based on key - common keys include work mode, data type, etc. + // Key values would need to be determined from SDK documentation + std::cout << " [param] key=0x" << std::hex << key + << " length=" << std::dec << length << "\n"; + + offset += length; + } + } +} + // -------------------------------------------------------------------------- // Callback: called by the SDK whenever a lidar is discovered or its info // changes (multiple calls for the same device are de-duplicated) @@ -88,25 +198,38 @@ static void OnLidarInfoChange(const uint32_t handle, void* /*client_data*/) { if (!info) return; - std::lock_guard lock(g_scan.mtx); + bool should_query = false; + { + std::lock_guard lock(g_scan.mtx); - // De-duplicate by handle - for (const auto& d : g_scan.devices) { - if (d.handle == handle) return; - } + // De-duplicate by handle + for (const auto& d : g_scan.devices) { + if (d.handle == handle) return; + } - DeviceEntry entry; - entry.handle = handle; - entry.lidar_ip = info->lidar_ip; - entry.sn = info->sn; - entry.dev_type = info->dev_type; + DeviceEntry entry; + entry.handle = handle; + entry.lidar_ip = info->lidar_ip; + entry.sn = info->sn; + entry.dev_type = info->dev_type; - std::cout << "[found] " << entry.lidar_ip - << " type=" << DevTypeName(entry.dev_type) - << " sn=" << entry.sn - << "\n"; + std::cout << "[found] " << entry.lidar_ip + << " type=" << DevTypeName(entry.dev_type) + << " sn=" << entry.sn + << "\n"; - g_scan.devices.push_back(std::move(entry)); + g_scan.devices.push_back(std::move(entry)); + should_query = true; + } + + // Query device configuration outside the lock + if (should_query) { + // Query firmware version + QueryLivoxLidarFirmwareVer(handle, OnFirmwareVersionQuery, nullptr); + + // Query internal info (includes various config parameters) + QueryLivoxLidarInternalInfo(handle, OnInternalInfoQuery, nullptr); + } } // -------------------------------------------------------------------------- @@ -267,7 +390,7 @@ int main(int argc, char** argv) { std::cout << "[result] Found " << devs.size() << " device(s):\n\n"; - // Column widths + // Print basic info table const int w_ip = 16; const int w_type = 16; const int w_sn = 18; @@ -296,5 +419,41 @@ int main(int argc, char** argv) { hline(); std::cout << "\n"; + // Print detailed configuration for each device + std::cout << "=== Device Configuration Details ===\n\n"; + + for (const auto& d : devs) { + std::cout << "Device: " << d.lidar_ip << " (" << d.sn << ")\n"; + std::cout << " Type: " << DevTypeName(d.dev_type) << "\n"; + + if (!d.config.firmware_version.empty()) { + std::cout << " Firmware Version: " << d.config.firmware_version << "\n"; + } + + if (d.config.pcl_data_type >= 0) { + std::cout << " Point Data Type: " << DataTypeName(d.config.pcl_data_type) + << " (" << d.config.pcl_data_type << ")\n"; + } + + if (d.config.scan_pattern >= 0) { + std::cout << " Scan Pattern: " << ScanPatternName(d.config.scan_pattern) + << " (" << d.config.scan_pattern << ")\n"; + } + + if (d.config.blind_spot > 0) { + std::cout << " Blind Spot: " << d.config.blind_spot << " cm\n"; + } + + std::cout << " Dual Emit: " << (d.config.dual_emit ? "Enabled" : "Disabled") << "\n"; + std::cout << " IMU Data: " << (d.config.imu_enabled ? "Enabled" : "Disabled") << "\n"; + + if (!d.config.work_mode.empty()) { + std::cout << " Work Mode: " << d.config.work_mode << "\n"; + } + + std::cout << " Config Queried: " << (d.config.config_queried ? "Yes" : "No") << "\n"; + std::cout << "\n"; + } + return 0; } From aa988678fb8fa14f0de2547b34ad552ee6d3d5dc Mon Sep 17 00:00:00 2001 From: ibrahim Date: Sun, 22 Mar 2026 19:39:26 +0000 Subject: [PATCH 5/7] feat: add UDP diagnostic script and enhance livox_scan with network configuration reporting --- src/tools/debug.py | 296 ++++++++++++++++++++++++++++++++++++++ src/tools/livox_scan.cpp | 297 +++++++++++++++++++++++++++++++++------ 2 files changed, 548 insertions(+), 45 deletions(-) create mode 100644 src/tools/debug.py diff --git a/src/tools/debug.py b/src/tools/debug.py new file mode 100644 index 00000000..878567e9 --- /dev/null +++ b/src/tools/debug.py @@ -0,0 +1,296 @@ +#!/usr/bin/env python3 +""" +UDP Diagnostic Script +Analyses UDP reception issues by testing at multiple levels simultaneously. +Usage: sudo python3 udp_diag.py --ip 192.168.1.10 --port [--iface eno1] [--duration 15] +""" + +import argparse +import socket +import subprocess +import threading +import time +import sys +import os +import struct +from datetime import datetime + + +# ────────────────────────────────────────────── +# Helpers +# ────────────────────────────────────────────── + +def header(title): + print(f"\n{'='*60}") + print(f" {title}") + print(f"{'='*60}") + +def run(cmd): + try: + result = subprocess.run(cmd, shell=True, capture_output=True, text=True, timeout=10) + return result.stdout.strip(), result.stderr.strip() + except subprocess.TimeoutExpired: + return "", "timeout" + +def ts(): + return datetime.now().strftime("%H:%M:%S.%f")[:-3] + + +# ────────────────────────────────────────────── +# 1. Static network diagnostics +# ────────────────────────────────────────────── + +def check_network(ip, iface): + header("1. NETWORK CONFIGURATION") + + print("\n--- Interface addresses ---") + out, _ = run(f"ip addr show {iface}") + print(out or f" [interface {iface} not found]") + + print("\n--- Routing table (relevant entries) ---") + out, _ = run("ip route show") + for line in out.splitlines(): + if "192.168.1" in line or "default" in line: + print(f" {line}") + + print(f"\n--- Route used to reach device 192.168.1.167 ---") + out, _ = run("ip route get 192.168.1.167") + print(f" {out}") + + print(f"\n--- Route used to reach device 192.168.1.184 ---") + out, _ = run("ip route get 192.168.1.184") + print(f" {out}") + + print(f"\n--- rp_filter settings ---") + for key in ["all", iface, "wlp1s0"]: + out, _ = run(f"sysctl net.ipv4.conf.{key}.rp_filter") + print(f" {out}") + + print(f"\n--- ARP cache for devices ---") + out, _ = run("arp -n") + for line in out.splitlines(): + if "192.168.1.16" in line or "192.168.1.18" in line: + print(f" {line}") + if not any(x in out for x in ["192.168.1.16", "192.168.1.18"]): + print(" [no ARP entries found for devices — try pinging them first]") + + +# ────────────────────────────────────────────── +# 2. Socket state check +# ────────────────────────────────────────────── + +def check_sockets(ip, port): + header("2. SOCKET STATE") + + print(f"\n--- All UDP sockets bound to {ip} or 0.0.0.0 ---") + out, _ = run("ss -ulnp") + found = False + print(f" {'Local Address':30s} {'Process'}") + for line in out.splitlines(): + if ip in line or "0.0.0.0" in line or "*" in line: + if "State" in line or "Recv" in line: # header + continue + print(f" {line}") + found = True + if not found: + print(" [no relevant UDP sockets found]") + + if port: + print(f"\n--- Checking specifically for port {port} ---") + out, _ = run(f"ss -ulnp sport = :{port}") + print(out or f" [nothing listening on UDP port {port}]") + + +# ────────────────────────────────────────────── +# 3. tcpdump capture (background thread) +# ────────────────────────────────────────────── + +tcpdump_results = [] + +def run_tcpdump(iface, port, duration): + port_filter = f"and udp port {port}" if port else "and udp" + cmd = ( + f"tcpdump -i {iface} -n -c 50 -tt " + f"'(src 192.168.1.167 or src 192.168.1.184) {port_filter}' " + f"2>&1" + ) + try: + proc = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, text=True) + time.sleep(duration) + proc.terminate() + out = proc.stdout.read() + tcpdump_results.extend(out.splitlines()) + except Exception as e: + tcpdump_results.append(f"tcpdump error: {e}") + + +# ────────────────────────────────────────────── +# 4. Raw socket capture (no UDP stack involvement) +# ────────────────────────────────────────────── + +raw_results = [] + +def run_raw_socket(iface, duration): + """ + Captures at raw Ethernet level — bypasses routing, iptables INPUT, + and socket binding entirely. If we see packets here but not on the + UDP socket, the problem is above the NIC driver. + """ + try: + s = socket.socket(socket.AF_PACKET, socket.SOCK_RAW, socket.htons(0x0800)) + s.bind((iface, 0)) + s.settimeout(1.0) + deadline = time.time() + duration + count = 0 + while time.time() < deadline: + try: + pkt = s.recv(65535) + # Parse IP header (starts at byte 14 after Ethernet header) + if len(pkt) < 34: + continue + proto = pkt[23] + if proto != 17: # UDP only + continue + src_ip = socket.inet_ntoa(pkt[26:30]) + dst_ip = socket.inet_ntoa(pkt[30:34]) + if src_ip in ("192.168.1.167", "192.168.1.184"): + dst_port = struct.unpack("!H", pkt[36:38])[0] + src_port = struct.unpack("!H", pkt[34:36])[0] + length = struct.unpack("!H", pkt[38:40])[0] + raw_results.append( + f" [{ts()}] UDP {src_ip}:{src_port} -> {dst_ip}:{dst_port} " + f"payload={length-8}B" + ) + count += 1 + if count >= 20: + break + except socket.timeout: + continue + s.close() + except PermissionError: + raw_results.append(" [raw socket requires root — run with sudo]") + except Exception as e: + raw_results.append(f" [raw socket error: {e}]") + + +# ────────────────────────────────────────────── +# 5. UDP socket test listener +# ────────────────────────────────────────────── + +udp_results = [] + +def run_udp_listener(ip, port, duration): + if not port: + udp_results.append(" [no port specified — skipping UDP listener test]") + return + try: + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + s.bind((ip, port)) + s.settimeout(1.0) + udp_results.append(f" Socket bound to {ip}:{port} — listening...") + deadline = time.time() + duration + count = 0 + while time.time() < deadline: + try: + data, addr = s.recvfrom(65535) + udp_results.append( + f" [{ts()}] Received {len(data)}B from {addr[0]}:{addr[1]}" + ) + count += 1 + if count >= 20: + break + except socket.timeout: + continue + if count == 0: + udp_results.append(f" [no UDP packets received on {ip}:{port} in {duration}s]") + s.close() + except OSError as e: + udp_results.append(f" [socket error: {e}]") + + +# ────────────────────────────────────────────── +# Main +# ────────────────────────────────────────────── + +def main(): + parser = argparse.ArgumentParser(description="UDP reception diagnostics") + parser.add_argument("--ip", default="192.168.1.10", help="Local IP to bind to") + parser.add_argument("--port", type=int, default=None, help="UDP port to test") + parser.add_argument("--iface", default="eno1", help="Ethernet interface name") + parser.add_argument("--duration", type=int, default=15, help="Capture duration in seconds") + args = parser.parse_args() + + if os.geteuid() != 0: + print("WARNING: Not running as root. Raw socket and tcpdump tests will fail.") + print(" Re-run with: sudo python3 udp_diag.py ...\n") + + print(f"\nUDP Diagnostic Tool") + print(f" Target IP : {args.ip}") + print(f" Port : {args.port or 'not specified'}") + print(f" Interface : {args.iface}") + print(f" Duration : {args.duration}s") + + # Static checks first (instant) + check_network(args.ip, args.iface) + check_sockets(args.ip, args.port) + + # Launch parallel capture threads + header(f"3. LIVE CAPTURE ({args.duration}s)") + print(f"\n Starting parallel captures — please ensure devices are streaming...\n") + + t_tcp = threading.Thread(target=run_tcpdump, args=(args.iface, args.port, args.duration)) + t_raw = threading.Thread(target=run_raw_socket, args=(args.iface, args.duration)) + t_udp = threading.Thread(target=run_udp_listener, args=(args.ip, args.port, args.duration)) + + t_tcp.start(); t_raw.start(); t_udp.start() + t_tcp.join(); t_raw.join(); t_udp.join() + + print("\n--- tcpdump (NIC via libpcap) ---") + if tcpdump_results: + for line in tcpdump_results: + print(f" {line}") + else: + print(" [no output]") + + print("\n--- Raw IP socket (kernel receive path, pre-iptables) ---") + for line in raw_results: + print(line) + + print(f"\n--- UDP socket bound to {args.ip}:{args.port or '?'} ---") + for line in udp_results: + print(line) + + # ── Interpretation ── + header("4. INTERPRETATION") + + saw_tcpdump = any("192.168.1" in l for l in tcpdump_results) + saw_raw = len([l for l in raw_results if "UDP" in l]) > 0 + saw_udp = any("Received" in l for l in udp_results) + + print() + if not saw_tcpdump and not saw_raw: + print(" ✗ No packets seen at NIC level (tcpdump + raw socket both empty)") + print(" → Devices are not sending, or sending to wrong IP/port") + print(" → Check device configuration and run: sudo tcpdump -i eno1 -n host 192.168.1.167") + elif saw_tcpdump and saw_raw and not saw_udp: + print(" ✓ Packets arriving at NIC") + print(" ✗ Not reaching UDP socket") + print(" → Check iptables INPUT rules, or port mismatch between device and listener") + elif saw_tcpdump and not saw_raw: + print(" ✓ tcpdump sees packets") + print(" ✗ Raw socket does not — unusual, may indicate interface name mismatch") + elif saw_udp: + print(" ✓ Packets received successfully on UDP socket!") + print(" → No reception problem detected during this run") + else: + print(" ✗ No packets seen at any level") + print(" → Verify devices are actively streaming during the test window") + + print() + + +if __name__ == "__main__": + main() diff --git a/src/tools/livox_scan.cpp b/src/tools/livox_scan.cpp index 535e6a74..3595ed4d 100644 --- a/src/tools/livox_scan.cpp +++ b/src/tools/livox_scan.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -42,15 +43,48 @@ // Per-device information collected during the scan // -------------------------------------------------------------------------- +struct NetworkConfig { + std::string lidar_ip; + std::string lidar_netmask; + std::string lidar_gateway; + + std::string point_host_ip; + uint16_t point_host_port; + uint16_t point_lidar_port; + + std::string imu_host_ip; + uint16_t imu_host_port; + uint16_t imu_lidar_port; + + std::string state_host_ip; + uint16_t state_host_port; + uint16_t state_lidar_port; + + std::string ctl_host_ip; + uint16_t ctl_host_port; + uint16_t ctl_lidar_port; + + std::string log_host_ip; + uint16_t log_host_port; + uint16_t log_lidar_port; + + NetworkConfig() : point_host_port(0), point_lidar_port(0), + imu_host_port(0), imu_lidar_port(0), + state_host_port(0), state_lidar_port(0), + ctl_host_port(0), ctl_lidar_port(0), + log_host_port(0), log_lidar_port(0) {} +}; + struct ConfigInfo { - std::string firmware_version; - std::string work_mode; - int32_t pcl_data_type; - int32_t scan_pattern; - uint32_t blind_spot; - bool dual_emit; - bool imu_enabled; - bool config_queried; + std::string firmware_version; + std::string work_mode; + int32_t pcl_data_type; + int32_t scan_pattern; + uint32_t blind_spot; + bool dual_emit; + bool imu_enabled; + bool config_queried; + NetworkConfig network; ConfigInfo() : pcl_data_type(-1), scan_pattern(-1), blind_spot(0), dual_emit(false), imu_enabled(false), config_queried(false) {} @@ -69,8 +103,9 @@ struct DeviceEntry { // -------------------------------------------------------------------------- struct ScanState { - std::mutex mtx; + std::mutex mtx; std::vector devices; + std::atomic pending_queries{0}; }; static ScanState g_scan; @@ -137,17 +172,23 @@ static void OnFirmwareVersionQuery(livox_status status, LivoxLidarDiagInternalInfoResponse* response, void* /*client_data*/) { DeviceEntry* dev = FindDeviceByHandle(handle); - if (!dev) return; - - if (status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) { - // Parse firmware version from response data - // The format is typically a param_num followed by key-value pairs - if (response->param_num > 0 && response->data[0]) { - // Simple extraction - firmware version is typically a string - dev->config.firmware_version = std::string(reinterpret_cast(response->data)); + + if (dev) { + if (status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) { + // Parse firmware version from response data + // The format is typically a param_num followed by key-value pairs + if (response->param_num > 0 && response->data[0]) { + // Simple extraction - firmware version is typically a string + dev->config.firmware_version = std::string(reinterpret_cast(response->data)); + } } + + std::cout << "[info] Firmware query response for " << dev->lidar_ip + << " (status=" << status << ")\n"; } - dev->config.config_queried = true; + + g_scan.pending_queries--; + std::cout << "[info] Pending queries: " << g_scan.pending_queries.load() << "\n"; } // -------------------------------------------------------------------------- @@ -158,35 +199,132 @@ static void OnInternalInfoQuery(livox_status status, LivoxLidarDiagInternalInfoResponse* response, void* /*client_data*/) { DeviceEntry* dev = FindDeviceByHandle(handle); - if (!dev) return; + + if (!dev) { + std::cout << "[error] Internal info callback for unknown device handle=" << handle << "\n"; + g_scan.pending_queries--; + return; + } + + if (status != kLivoxLidarStatusSuccess || response == nullptr || response->ret_code != 0) { + std::cout << "[warn] Internal config query failed for " << dev->lidar_ip + << " (status=" << status; + if (response) std::cout << ", ret_code=" << (int)response->ret_code; + std::cout << ")\n"; + g_scan.pending_queries--; + return; + } - if (status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) { - std::cout << "[info] Received internal config for " << dev->lidar_ip - << " (param_num=" << response->param_num << ")\n"; - - // Parse key-value parameters from the response - // The data format is: [key(2B)][length(2B)][value(length B)] repeated - uint8_t* ptr = response->data; - size_t offset = 0; - - for (int i = 0; i < response->param_num && offset < 1024; ++i) { - if (offset + 4 > 1024) break; - - uint16_t key = *reinterpret_cast(ptr + offset); - offset += 2; - uint16_t length = *reinterpret_cast(ptr + offset); - offset += 2; - - if (offset + length > 1024) break; - - // Parse based on key - common keys include work mode, data type, etc. - // Key values would need to be determined from SDK documentation - std::cout << " [param] key=0x" << std::hex << key - << " length=" << std::dec << length << "\n"; - - offset += length; + dev->config.config_queried = true; + + // Iterate key-value pairs using the LivoxLidarKeyValueParam struct, exactly + // as shown in the Livox SDK2 sample (livox_lidar_quick_start/main.cpp). + // Each entry: key(2B) + length(2B) + value(length B). + uint16_t off = 0; + for (uint8_t i = 0; i < response->param_num; ++i) { + LivoxLidarKeyValueParam* kv = reinterpret_cast(&response->data[off]); + + // Helper: format 4-byte little-endian IP stored in kv->value[idx..idx+3] + auto parse_ip = [&](int idx) -> std::string { + char buf[20]; + snprintf(buf, sizeof(buf), "%u.%u.%u.%u", + (uint8_t)kv->value[idx], (uint8_t)kv->value[idx+1], + (uint8_t)kv->value[idx+2], (uint8_t)kv->value[idx+3]); + return buf; + }; + + switch (kv->key) { + case kKeyPclDataType: + if (kv->length >= 1) dev->config.pcl_data_type = kv->value[0]; + break; + + case kKeyPatternMode: + if (kv->length >= 1) dev->config.scan_pattern = kv->value[0]; + break; + + case kKeyDualEmitEn: + if (kv->length >= 1) dev->config.dual_emit = (kv->value[0] != 0); + break; + + case kKeyImuDataEn: + if (kv->length >= 1) dev->config.imu_enabled = (kv->value[0] != 0); + break; + + case kKeyBlindSpotSet: + if (kv->length >= 4) memcpy(&dev->config.blind_spot, &kv->value[0], 4); + break; + + case kKeyWorkMode: + if (kv->length >= 1) { + switch (kv->value[0]) { + case 0x01: dev->config.work_mode = "Normal"; break; + case 0x02: dev->config.work_mode = "WakeUp"; break; + case 0x03: dev->config.work_mode = "Sleep"; break; + default: dev->config.work_mode = "Unknown"; break; + } + } + break; + + // LiDAR IP config: ip[4] + netmask[4] + gateway[4] (12 bytes on wire) + case kKeyLidarIpCfg: + if (kv->length >= 12) { + dev->config.network.lidar_ip = parse_ip(0); + dev->config.network.lidar_netmask = parse_ip(4); + dev->config.network.lidar_gateway = parse_ip(8); + } + break; + + // Host IP configs: ip[4] + host_port[2] + lidar_port[2] (8 bytes on wire) + case kKeyStateInfoHostIpCfg: + if (kv->length >= 8) { + dev->config.network.state_host_ip = parse_ip(0); + memcpy(&dev->config.network.state_host_port, &kv->value[4], 2); + memcpy(&dev->config.network.state_lidar_port, &kv->value[6], 2); + } + break; + + case kKeyLidarPointDataHostIpCfg: + if (kv->length >= 8) { + dev->config.network.point_host_ip = parse_ip(0); + memcpy(&dev->config.network.point_host_port, &kv->value[4], 2); + memcpy(&dev->config.network.point_lidar_port, &kv->value[6], 2); + } + break; + + case kKeyLidarImuHostIpCfg: + if (kv->length >= 8) { + dev->config.network.imu_host_ip = parse_ip(0); + memcpy(&dev->config.network.imu_host_port, &kv->value[4], 2); + memcpy(&dev->config.network.imu_lidar_port, &kv->value[6], 2); + } + break; + + case kKeyCtlHostIpCfg: + if (kv->length >= 8) { + dev->config.network.ctl_host_ip = parse_ip(0); + memcpy(&dev->config.network.ctl_host_port, &kv->value[4], 2); + memcpy(&dev->config.network.ctl_lidar_port, &kv->value[6], 2); + } + break; + + case kKeyLogHostIpCfg: + if (kv->length >= 8) { + dev->config.network.log_host_ip = parse_ip(0); + memcpy(&dev->config.network.log_host_port, &kv->value[4], 2); + memcpy(&dev->config.network.log_lidar_port, &kv->value[6], 2); + } + break; + + default: + break; } + + off += sizeof(uint16_t) * 2; // key(2) + length(2) + off += kv->length; } + + g_scan.pending_queries--; + std::cout << "[info] Pending queries: " << g_scan.pending_queries.load() << "\n"; } // -------------------------------------------------------------------------- @@ -224,6 +362,11 @@ static void OnLidarInfoChange(const uint32_t handle, // Query device configuration outside the lock if (should_query) { + std::cout << "[info] Querying configuration for " << info->lidar_ip << " ...\n"; + + // Increment counter before making queries + g_scan.pending_queries += 2; + // Query firmware version QueryLivoxLidarFirmwareVer(handle, OnFirmwareVersionQuery, nullptr); @@ -367,6 +510,23 @@ int main(int argc, char** argv) { std::this_thread::sleep_for(std::chrono::seconds(timeout_secs)); + // Wait for pending queries to complete (with timeout) + int wait_count = 0; + const int max_wait = 50; // 5 seconds max + while (g_scan.pending_queries.load() > 0 && wait_count < max_wait) { + std::cout << "[info] Waiting for " << g_scan.pending_queries.load() + << " pending queries...\n"; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + wait_count++; + } + + if (g_scan.pending_queries.load() > 0) { + std::cout << "[warn] Timed out waiting for queries, " + << g_scan.pending_queries.load() << " still pending\n"; + } else { + std::cout << "[info] All queries completed\n"; + } + LivoxLidarSdkUninit(); // Remove the temporary config file written to avoid the SDK SIGSEGV @@ -451,7 +611,54 @@ int main(int argc, char** argv) { std::cout << " Work Mode: " << d.config.work_mode << "\n"; } - std::cout << " Config Queried: " << (d.config.config_queried ? "Yes" : "No") << "\n"; + // Network Configuration + std::cout << "\n Network Configuration:\n"; + + if (!d.config.network.lidar_ip.empty()) { + std::cout << " LiDAR IP: " << d.config.network.lidar_ip << "\n"; + if (!d.config.network.lidar_netmask.empty()) { + std::cout << " Netmask: " << d.config.network.lidar_netmask << "\n"; + } + if (!d.config.network.lidar_gateway.empty()) { + std::cout << " Gateway: " << d.config.network.lidar_gateway << "\n"; + } + } + + if (!d.config.network.point_host_ip.empty()) { + std::cout << "\n Point Cloud:\n"; + std::cout << " Host IP: " << d.config.network.point_host_ip << "\n"; + std::cout << " Host Port: " << d.config.network.point_host_port << "\n"; + std::cout << " LiDAR Port: " << d.config.network.point_lidar_port << "\n"; + } + + if (!d.config.network.imu_host_ip.empty()) { + std::cout << "\n IMU Data:\n"; + std::cout << " Host IP: " << d.config.network.imu_host_ip << "\n"; + std::cout << " Host Port: " << d.config.network.imu_host_port << "\n"; + std::cout << " LiDAR Port: " << d.config.network.imu_lidar_port << "\n"; + } + + if (!d.config.network.state_host_ip.empty()) { + std::cout << "\n State Info:\n"; + std::cout << " Host IP: " << d.config.network.state_host_ip << "\n"; + std::cout << " Host Port: " << d.config.network.state_host_port << "\n"; + std::cout << " LiDAR Port: " << d.config.network.state_lidar_port << "\n"; + } + + if (!d.config.network.ctl_host_ip.empty()) { + std::cout << "\n Control:\n"; + std::cout << " Host IP: " << d.config.network.ctl_host_ip << "\n"; + std::cout << " Host Port: " << d.config.network.ctl_host_port << "\n"; + std::cout << " LiDAR Port: " << d.config.network.ctl_lidar_port << "\n"; + } + + if (!d.config.network.log_host_ip.empty()) { + std::cout << "\n Logging:\n"; + std::cout << " Host IP: " << d.config.network.log_host_ip << "\n"; + std::cout << " Host Port: " << d.config.network.log_host_port << "\n"; + std::cout << " LiDAR Port: " << d.config.network.log_lidar_port << "\n"; + } + std::cout << "\n"; } From 7f8ce52f212320b5a6ddce22ca67195a7d67410b Mon Sep 17 00:00:00 2001 From: ibrahim Date: Sun, 22 Mar 2026 19:43:17 +0000 Subject: [PATCH 6/7] fix: improve firmware version parsing in OnFirmwareVersionQuery function --- src/tools/livox_scan.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/tools/livox_scan.cpp b/src/tools/livox_scan.cpp index 3595ed4d..185d0652 100644 --- a/src/tools/livox_scan.cpp +++ b/src/tools/livox_scan.cpp @@ -173,22 +173,26 @@ static void OnFirmwareVersionQuery(livox_status status, void* /*client_data*/) { DeviceEntry* dev = FindDeviceByHandle(handle); - if (dev) { - if (status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) { - // Parse firmware version from response data - // The format is typically a param_num followed by key-value pairs - if (response->param_num > 0 && response->data[0]) { - // Simple extraction - firmware version is typically a string - dev->config.firmware_version = std::string(reinterpret_cast(response->data)); + if (dev && status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) { + uint16_t off = 0; + for (uint8_t i = 0; i < response->param_num; ++i) { + LivoxLidarKeyValueParam* kv = reinterpret_cast(&response->data[off]); + if (kv->key == kKeyVersionApp && kv->length >= 4) { + // version_app is uint8_t[4]: [major, minor, patch, build] + char buf[32]; + snprintf(buf, sizeof(buf), "%u.%u.%u.%u", + (uint8_t)kv->value[0], (uint8_t)kv->value[1], + (uint8_t)kv->value[2], (uint8_t)kv->value[3]); + dev->config.firmware_version = buf; + break; } + off += sizeof(uint16_t) * 2 + kv->length; } - std::cout << "[info] Firmware query response for " << dev->lidar_ip - << " (status=" << status << ")\n"; + << ": " << dev->config.firmware_version << "\n"; } g_scan.pending_queries--; - std::cout << "[info] Pending queries: " << g_scan.pending_queries.load() << "\n"; } // -------------------------------------------------------------------------- From f3b3a2497ed0804b7c9547d203e5b867466f68a7 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 23 Mar 2026 08:02:22 +0000 Subject: [PATCH 7/7] fix: per-lidar frame IDs, fix launch descriptions, consolidate CLI into livox tool with firmware_upgrade Co-authored-by: marc-hanheide <1153084+marc-hanheide@users.noreply.github.com> Agent-Logs-Url: https://github.com/jabasai/livox_ros_driver2/sessions/fd894be1-58c0-4ec6-8e86-b7f30cf25cf3 --- CMakeLists.txt | 35 +- README.md | 138 ++---- launch/dual_mid360_launch.py | 4 +- launch/mid360_launch.py | 2 +- src/lddc.cpp | 15 + src/lddc.h | 25 + src/livox_ros_driver2.cpp | 31 ++ src/tools/livox.cpp | 870 +++++++++++++++++++++++++++++++++++ 8 files changed, 1001 insertions(+), 119 deletions(-) create mode 100644 src/tools/livox.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e084c0f..f0ef6f4c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -338,39 +338,16 @@ else(ROS_EDITION STREQUAL "ROS2") EXECUTABLE ${PROJECT_NAME}_node ) - # livox_set_ip: standalone command-line tool to configure lidar IP addresses - add_executable(livox_set_ip src/tools/livox_set_ip.cpp) - target_include_directories(livox_set_ip PRIVATE + # livox: unified command-line tool (search, setup, reboot, reset, firmware_upgrade) + # Replaces the former livox_set_ip, livox_scan, and livox_reboot tools. + add_executable(livox src/tools/livox.cpp) + target_include_directories(livox PRIVATE ${LIVOX_LIDAR_SDK_INCLUDE_DIR} ) - target_link_libraries(livox_set_ip + target_link_libraries(livox ${LIVOX_LIDAR_SDK_LIBRARY} ) - install(TARGETS livox_set_ip - DESTINATION lib/${PROJECT_NAME} - ) - - # livox_scan: standalone command-line tool to discover and report Livox devices - add_executable(livox_scan src/tools/livox_scan.cpp) - target_include_directories(livox_scan PRIVATE - ${LIVOX_LIDAR_SDK_INCLUDE_DIR} - ) - target_link_libraries(livox_scan - ${LIVOX_LIDAR_SDK_LIBRARY} - ) - install(TARGETS livox_scan - DESTINATION lib/${PROJECT_NAME} - ) - - # livox_reboot: standalone command-line tool to reboot or reset Livox devices - add_executable(livox_reboot src/tools/livox_reboot.cpp) - target_include_directories(livox_reboot PRIVATE - ${LIVOX_LIDAR_SDK_INCLUDE_DIR} - ) - target_link_libraries(livox_reboot - ${LIVOX_LIDAR_SDK_LIBRARY} - ) - install(TARGETS livox_reboot + install(TARGETS livox DESTINATION lib/${PROJECT_NAME} ) diff --git a/README.md b/README.md index 75862297..4bd51f7c 100644 --- a/README.md +++ b/README.md @@ -614,131 +614,93 @@ For more infomation about the HAP config, please refer to: ## 5. Utility Tools -### 5.1 livox_set_ip — Change a LiDAR's IP address - -`livox_set_ip` is a standalone command-line tool that uses Livox-SDK2 to assign a new static IP address to any connected Livox LiDAR (e.g. MID-360, HAP). It is useful for initial device setup or when you need to move a LiDAR to a different subnet. - -**Usage:** +The `livox` CLI is a single unified command-line tool that consolidates device management operations. Run it with: ```shell -ros2 run livox_ros_driver2 livox_set_ip [host_ip] +ros2 run livox_ros_driver2 livox [options] ``` -| Argument | Description | +| Verb | Description | | --- | --- | -| `current_lidar_ip` | Current IP of the LiDAR to configure | -| `new_ip` | New static IP to assign to the LiDAR | -| `netmask` | Subnet mask (e.g. `255.255.255.0`) | -| `gateway` | Gateway address (e.g. `192.168.1.1`) | -| `host_ip` | *(optional)* IP of the host NIC connected to the LiDAR; omit to let the SDK auto-detect | - -**Examples:** - -```shell -# Change lidar IP; host NIC is explicitly specified -ros2 run livox_ros_driver2 livox_set_ip 192.168.1.100 192.168.1.167 255.255.255.0 192.168.1.1 192.168.1.10 - -# Host IP auto-detected by the SDK -ros2 run livox_ros_driver2 livox_set_ip 192.168.1.167 192.168.1.184 255.255.255.0 192.168.1.1 -``` +| `search` | Discover all Livox devices visible on the network | +| `setup` | Assign a static IP address to a lidar | +| `reboot` | Soft-reboot one or more lidars (firmware restart) | +| `reset` | Reset one or more lidars (configuration/state reset) | +| `firmware_upgrade` | Upgrade the firmware on one or more lidars | -> **IMPORTANT:** Power-cycle the LiDAR after the tool reports success for the new IP address to take effect. +Run `livox --help` for per-verb usage. -### 5.2 livox_scan — Discover LiDAR devices on the network - -`livox_scan` listens for Livox device announcements for a configurable period and prints a summary table of every device found, including its IP address, device type, and serial number. - -**Usage:** +### 5.1 search — Discover LiDAR devices on the network ```shell -ros2 run livox_ros_driver2 livox_scan [OPTIONS] [host_ip [timeout_seconds]] +ros2 run livox_ros_driver2 livox search [host_ip [timeout_seconds]] ``` -**Options:** - -| Option | Description | -| --- | --- | -| `-h`, `--help` | Show help message and exit | - -**Arguments:** - -| Argument | Description | -| --- | --- | -| `host_ip` | *(optional)* IP of the host NIC facing the LiDARs; omit to let the SDK auto-detect | -| `timeout_seconds` | *(optional)* How long to scan in seconds (default: `10`) | - -**Examples:** - ```shell -# Show help -ros2 run livox_ros_driver2 livox_scan --help +# Scan for 10 seconds (default) +ros2 run livox_ros_driver2 livox search -# Scan all NICs for 10 seconds (default) -ros2 run livox_ros_driver2 livox_scan +# Scan from a specific NIC for 5 seconds +ros2 run livox_ros_driver2 livox search 192.168.1.10 5 +``` -# Scan from a specific NIC -ros2 run livox_ros_driver2 livox_scan 192.168.1.10 +### 5.2 setup — Change a LiDAR's IP address -# Scan from a specific NIC with a 5-second timeout -ros2 run livox_ros_driver2 livox_scan 192.168.1.10 5 +```shell +ros2 run livox_ros_driver2 livox setup [host_ip] ``` -**Example output:** +```shell +# Change lidar from factory IP to a static IP +ros2 run livox_ros_driver2 livox setup 192.168.1.100 192.168.1.167 255.255.255.0 192.168.1.1 +# With explicit host NIC address +ros2 run livox_ros_driver2 livox setup 192.168.1.100 192.168.1.167 255.255.255.0 192.168.1.1 192.168.1.10 ``` -=== Livox LiDAR scanner === - Host NIC : (auto-detect) - Timeout : 10 s -[info] Scanning for 10 second(s) ... +**IMPORTANT:** Power-cycle the lidar after this command for the new IP to take effect. -[found] 192.168.1.167 type=Mid-360 sn=0TFDH7600601234 -[found] 192.168.1.184 type=Mid-360 sn=0TFDH7600605678 +### 5.3 reboot / reset — Reboot or reset LiDAR devices -[result] Found 2 device(s): +```shell +ros2 run livox_ros_driver2 livox reboot [ip2 ...] [--host-ip ] +ros2 run livox_ros_driver2 livox reboot --all [--host-ip ] -+------------------+------------------+--------------------+ -| IP Address | Device Type | Serial Number | -+------------------+------------------+--------------------+ -| 192.168.1.167 | Mid-360 | 0TFDH7600601234 | -| 192.168.1.184 | Mid-360 | 0TFDH7600605678 | -+------------------+------------------+--------------------+ +ros2 run livox_ros_driver2 livox reset [ip2 ...] [--host-ip ] +ros2 run livox_ros_driver2 livox reset --all [--host-ip ] ``` -### 5.3 livox_reboot — Reboot or reset LiDAR devices +| Command | Effect | +| --- | --- | +| `reboot` | Soft **reboot** of the LiDAR firmware (equivalent to a power cycle) | +| `reset` | **Reset** the LiDAR (configuration/state reset, not a full reboot) | -`livox_reboot` sends a soft **reboot** (firmware-triggered restart) or **reset** (configuration/state reset) command to one or more Livox LiDARs over the network. It is useful for recovering from a hung state or applying settings that require a restart. +> ⚠️ `reboot` is a firmware-triggered soft restart over the network — it does **not** cut physical power. Allow ~30 seconds for the device to come back online. -> ⚠️ This is a **soft reboot over the network** — it does not cut physical power. Allow ~30 seconds for the device to come back online. +```shell +ros2 run livox_ros_driver2 livox reboot 192.168.1.167 +ros2 run livox_ros_driver2 livox reboot 192.168.1.167 192.168.1.184 +ros2 run livox_ros_driver2 livox reset --all +ros2 run livox_ros_driver2 livox reboot 192.168.1.167 --host-ip 192.168.1.10 +``` -**Usage:** +### 5.4 firmware_upgrade — Upgrade LiDAR firmware ```shell -ros2 run livox_ros_driver2 livox_reboot [ip2 ...] [--reset] [--host-ip ] -ros2 run livox_ros_driver2 livox_reboot --all [--reset] [--host-ip ] +ros2 run livox_ros_driver2 livox firmware_upgrade [ip1 ip2 ...] [--all] [--host-ip ] ``` -| Option | Description | -| --- | --- | -| `--reset` | Send a Reset command (config/state reset) instead of a full Reboot | -| `--host-ip ` | *(optional)* IP of the host NIC; auto-detected if omitted | -| `--all` | Target all discovered devices — use with caution! | - -**Examples:** +> ⚠️ Do **not** power off the device during an upgrade. The lidar reboots automatically on success. ```shell -# Reboot a single device -ros2 run livox_ros_driver2 livox_reboot 192.168.1.167 +# Upgrade a specific device +ros2 run livox_ros_driver2 livox firmware_upgrade /path/to/MID360_FW_v13.18.0244.bin 192.168.1.167 -# Reboot both lidars simultaneously -ros2 run livox_ros_driver2 livox_reboot 192.168.1.167 192.168.1.184 +# Upgrade all discovered devices simultaneously +ros2 run livox_ros_driver2 livox firmware_upgrade /path/to/firmware.bin --all +``` -# Reset (not reboot) all discovered devices -ros2 run livox_ros_driver2 livox_reboot --all --reset -# Reboot with an explicit host NIC address -ros2 run livox_ros_driver2 livox_reboot 192.168.1.167 --host-ip 192.168.1.10 -``` --- diff --git a/launch/dual_mid360_launch.py b/launch/dual_mid360_launch.py index 1a49187b..b7103988 100644 --- a/launch/dual_mid360_launch.py +++ b/launch/dual_mid360_launch.py @@ -198,7 +198,9 @@ def _launch_setup(context, *args, **kwargs): {'data_src': int(LaunchConfiguration('data_src').perform(context))}, {'publish_freq': float(LaunchConfiguration('publish_freq').perform(context))}, {'output_data_type': int(LaunchConfiguration('output_type').perform(context))}, - {'frame_id': front_frame_id}, + {'frame_id': front_frame_id}, # global default (used if no per-lidar match) + # Per-lidar frame_id overrides — each lidar uses its own TF frame: + {'lidar_frame_ids': f'{front_ip}:{front_frame_id},{back_ip}:{back_frame_id}'}, {'user_config_path': config_path}, {'cmdline_input_bd_code': ''}, {'connect_timeout_s': float(LaunchConfiguration('connect_timeout_s').perform(context))}, diff --git a/launch/mid360_launch.py b/launch/mid360_launch.py index eb8633d8..96cf42d4 100644 --- a/launch/mid360_launch.py +++ b/launch/mid360_launch.py @@ -275,7 +275,7 @@ def generate_launch_description(): default_value='', description=( 'TF frame ID for the lidar. ' - 'When empty (default): uses "_livox_frame" if namespace is set, ' + 'When empty (default): uses "_link" if namespace is set, ' 'otherwise "livox_frame".' ), ), diff --git a/src/lddc.cpp b/src/lddc.cpp index 4bd03ef9..07b32212 100644 --- a/src/lddc.cpp +++ b/src/lddc.cpp @@ -116,6 +116,17 @@ int Lddc::RegisterLds(Lds *lds) { } } +std::string Lddc::GetFrameId(uint8_t index) const { + if (lds_ && index < kMaxSourceLidar) { + uint32_t handle = lds_->lidars_[index].handle; + auto it = lidar_frame_ids_.find(handle); + if (it != lidar_frame_ids_.end() && !it->second.empty()) { + return it->second; + } + } + return frame_id_; +} + void Lddc::DistributePointCloudData(void) { if (!lds_) { LIVOX_ERROR("DistributePointCloudData: LDS not registered"); @@ -223,6 +234,7 @@ void Lddc::PublishPointcloud2(LidarDataQueue *queue, uint8_t index) { PointCloud2 cloud; uint64_t timestamp = 0; InitPointcloud2Msg(pkg, cloud, timestamp); + cloud.header.frame_id = GetFrameId(index); PublishPointcloud2Data(index, timestamp, cloud); } } @@ -238,6 +250,7 @@ void Lddc::PublishCustomPointcloud(LidarDataQueue *queue, uint8_t index) { CustomMsg livox_msg; InitCustomMsg(livox_msg, pkg, index); + livox_msg.header.frame_id = GetFrameId(index); FillPointsToCustomMsg(livox_msg, pkg); PublishCustomPointData(livox_msg, index); } @@ -265,6 +278,7 @@ void Lddc::PublishPclMsg(LidarDataQueue *queue, uint8_t index) { PointCloud cloud; uint64_t timestamp = 0; InitPclMsg(pkg, cloud, timestamp); + cloud.header.frame_id = GetFrameId(index); FillPointsToPclMsg(pkg, cloud); PublishPclData(index, timestamp, cloud); } @@ -511,6 +525,7 @@ void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index ImuMsg imu_msg; uint64_t timestamp; InitImuMsg(imu_data, imu_msg, timestamp); + imu_msg.header.frame_id = GetFrameId(index); // use per-lidar override (default was hardcoded "livox_frame") #ifdef BUILDING_ROS1 PublisherPtr publisher_ptr = GetCurrentImuPublisher(index); diff --git a/src/lddc.h b/src/lddc.h index 6d146d13..15575f95 100644 --- a/src/lddc.h +++ b/src/lddc.h @@ -26,6 +26,8 @@ #define LIVOX_ROS_DRIVER2_LDDC_H_ #include "include/livox_ros_driver2.h" +#include +#include #include "driver_node.h" #include "lds.h" @@ -90,6 +92,18 @@ class Lddc final { uint8_t IsMultiTopic(void) { return use_multi_topic_; } void SetRosNode(livox_ros::DriverNode *node) { cur_node_ = node; } + /** + * Register per-lidar frame IDs. + * + * @param ids Map of lidar handle (packed IPv4 uint32) → ROS frame_id string. + * When a handle is present in the map, its frame_id overrides the + * global frame_id_ for every message published from that lidar. + * Pass an empty map to clear all per-lidar overrides. + */ + void SetLidarFrameIds(std::unordered_map ids) { + lidar_frame_ids_ = std::move(ids); + } + // void SetRosPub(ros::Publisher *pub) { global_pub_ = pub; }; // NOT USED void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; } @@ -131,6 +145,14 @@ class Lddc final { PublisherPtr GetCurrentPublisher(uint8_t index); PublisherPtr GetCurrentImuPublisher(uint8_t index); + /** + * Return the effective ROS frame_id for lidar at position @p index. + * + * Looks up the lidar's handle in lidar_frame_ids_; falls back to the + * global frame_id_ if no per-lidar override is registered. + */ + std::string GetFrameId(uint8_t index) const; + private: uint8_t transfer_format_; uint8_t use_multi_topic_; @@ -139,6 +161,9 @@ class Lddc final { double publish_frq_; uint32_t publish_period_ns_; std::string frame_id_; + /** Per-lidar frame_id overrides (handle → frame_id). Populated via + * SetLidarFrameIds(); populated entries take precedence over frame_id_. */ + std::unordered_map lidar_frame_ids_; #ifdef BUILDING_ROS1 bool enable_lidar_bag_; diff --git a/src/livox_ros_driver2.cpp b/src/livox_ros_driver2.cpp index 19994f49..09dba449 100644 --- a/src/livox_ros_driver2.cpp +++ b/src/livox_ros_driver2.cpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include #include "include/livox_ros_driver2.h" #include "include/ros_headers.h" @@ -34,6 +36,7 @@ #include "driver_node.h" #include "lddc.h" #include "lds_lidar.h" +#include "comm/comm.h" using namespace livox_ros; @@ -139,6 +142,10 @@ DriverNode::DriverNode(const rclcpp::NodeOptions & node_options) this->declare_parameter("cmdline_input_bd_code", "000000000000001"); this->declare_parameter("lvx_file_path", "/home/livox/livox_test.lvx"); this->declare_parameter("connect_timeout_s", 10.0); + // Per-lidar frame_id overrides as "ip1:frame1,ip2:frame2,..." string. + // When set, the frame_id for each lidar is determined by its IP address + // rather than the single global frame_id parameter. + this->declare_parameter("lidar_frame_ids", std::string("")); this->get_parameter("xfer_format", xfer_format); this->get_parameter("multi_topic", multi_topic); @@ -161,6 +168,30 @@ DriverNode::DriverNode(const rclcpp::NodeOptions & node_options) lddc_ptr_ = std::make_unique(xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id); lddc_ptr_->SetRosNode(this); + // Parse per-lidar frame_id overrides ("ip1:frame1,ip2:frame2,...") + { + std::string lidar_frame_ids_str; + this->get_parameter("lidar_frame_ids", lidar_frame_ids_str); + if (!lidar_frame_ids_str.empty()) { + std::unordered_map frame_id_map; + std::istringstream ss(lidar_frame_ids_str); + std::string entry; + while (std::getline(ss, entry, ',')) { + auto colon = entry.find(':'); + if (colon != std::string::npos) { + std::string ip = entry.substr(0, colon); + std::string fid = entry.substr(colon + 1); + uint32_t handle = IpStringToNum(ip); + if (handle != 0) { + frame_id_map[handle] = fid; + DRIVER_INFO(*this, "Per-lidar frame_id: %s -> %s", ip.c_str(), fid.c_str()); + } + } + } + lddc_ptr_->SetLidarFrameIds(std::move(frame_id_map)); + } + } + if (data_src == kSourceRawLidar) { DRIVER_INFO(*this, "Data Source is raw lidar."); diff --git a/src/tools/livox.cpp b/src/tools/livox.cpp new file mode 100644 index 00000000..e666538e --- /dev/null +++ b/src/tools/livox.cpp @@ -0,0 +1,870 @@ +/** + * livox -- Unified Livox LiDAR command-line tool + * + * Usage: + * livox [options] + * + * Verbs: + * search Discover all Livox devices visible on the network. + * setup Assign a static IP address to a lidar. + * reboot Soft-reboot one or more lidars (firmware restart). + * reset Reset one or more lidars (config/state reset). + * firmware_upgrade Upgrade the firmware on one or more lidars. + * + * Run livox --help for per-verb usage. + * + * Examples: + * livox search + * livox search 192.168.1.10 5 + * livox setup 192.168.1.111 192.168.1.167 255.255.255.0 192.168.1.1 + * livox reboot 192.168.1.167 + * livox reboot 192.168.1.167 192.168.1.184 + * livox reset --all + * livox firmware_upgrade /path/to/firmware.bin 192.168.1.167 + * livox firmware_upgrade /path/to/firmware.bin --all + * + * Build: + * See CMakeLists.txt + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// --------------------------------------------------------------------------- +// Common helpers +// --------------------------------------------------------------------------- + +static std::string HandleToIp(uint32_t handle) { + char buf[20]; + snprintf(buf, sizeof(buf), "%u.%u.%u.%u", + (handle >> 24) & 0xFF, + (handle >> 16) & 0xFF, + (handle >> 8) & 0xFF, + (handle ) & 0xFF); + return std::string(buf); +} + +static uint32_t IpToHandle(const std::string& ip) { + // inet_addr returns IP in network byte order; the SDK handle uses the same + // encoding that HandleToIp() above decodes, which is big-endian uint32. + // On little-endian hosts inet_addr and HandleToIp are consistent. + struct in_addr addr{}; + if (inet_pton(AF_INET, ip.c_str(), &addr) != 1) return 0; + uint32_t n = ntohl(addr.s_addr); + return n; +} + +static std::string WriteTempConfig(const std::string& host_ip) { + std::string path = "/tmp/livox_tool_config_" + std::to_string(getpid()) + ".json"; + const std::string ip = host_ip.empty() ? "0.0.0.0" : host_ip; + + std::ofstream f(path); + if (!f.is_open()) return ""; + + f << "{\n" + << " \"lidar_summary_info\": { \"lidar_type\": 8 },\n" + << " \"MID360\": {\n" + << " \"lidar_net_info\": {\n" + << " \"cmd_data_port\": 56100,\n" + << " \"push_msg_port\": 56200,\n" + << " \"point_data_port\": 56300,\n" + << " \"imu_data_port\": 56400,\n" + << " \"log_data_port\": 56500\n" + << " },\n" + << " \"host_net_info\": {\n" + << " \"cmd_data_ip\": \"" << ip << "\",\n" + << " \"cmd_data_port\": 56101,\n" + << " \"push_msg_ip\": \"" << ip << "\",\n" + << " \"push_msg_port\": 56201,\n" + << " \"point_data_ip\": \"" << ip << "\",\n" + << " \"point_data_port\": 56301,\n" + << " \"imu_data_ip\": \"" << ip << "\",\n" + << " \"imu_data_port\": 56401,\n" + << " \"log_data_ip\": \"\",\n" + << " \"log_data_port\": 56501\n" + << " }\n" + << " },\n" + << " \"lidar_configs\": []\n" + << "}\n"; + + return f.good() ? path : ""; +} + +static bool InitSdk(const std::string& host_ip, + LivoxLidarInfoChangeCallback info_cb, void* cb_data, + std::string& out_tmp_cfg) { + DisableLivoxSdkConsoleLogger(); + SetLivoxLidarInfoChangeCallback(info_cb, cb_data); + + out_tmp_cfg = WriteTempConfig(host_ip); + if (out_tmp_cfg.empty()) { + std::cerr << "[warn] Could not write temporary config; SDK may crash on discovery.\n"; + } + + const char* cfg = out_tmp_cfg.empty() ? nullptr : out_tmp_cfg.c_str(); + const char* host = host_ip.empty() ? nullptr : host_ip.c_str(); + + if (!LivoxLidarSdkInit(cfg, host)) { + std::cerr << "[err] LivoxLidarSdkInit() failed. " + "Check that the host NIC IP is correct.\n"; + if (!out_tmp_cfg.empty()) std::remove(out_tmp_cfg.c_str()); + out_tmp_cfg.clear(); + return false; + } + return true; +} + +static void CleanupSdk(const std::string& tmp_cfg) { + LivoxLidarSdkUninit(); + if (!tmp_cfg.empty()) std::remove(tmp_cfg.c_str()); +} + +// --------------------------------------------------------------------------- +// verb: search +// --------------------------------------------------------------------------- + +static const char* kSearchUsage = R"( +Usage: + livox search [host_ip [timeout_seconds]] + +Options: + host_ip NIC address to use for scanning (auto-detected if omitted) + timeout_seconds How long to scan (default: 10) + +Examples: + livox search + livox search 192.168.1.10 + livox search 192.168.1.10 5 +)"; + +struct DeviceEntry { + uint32_t handle; + std::string ip; + std::string sn; + uint8_t dev_type; +}; + +struct SearchState { + std::mutex mtx; + std::vector devices; +}; + +static SearchState g_search; + +static void OnSearchLidarInfo(const uint32_t handle, + const LivoxLidarInfo* info, + void* /*client_data*/) { + if (!info) return; + std::lock_guard lock(g_search.mtx); + for (const auto& d : g_search.devices) + if (d.handle == handle) return; // already known + + DeviceEntry e; + e.handle = handle; + e.ip = info->lidar_ip; + e.sn = info->sn; + e.dev_type = info->dev_type; + g_search.devices.push_back(e); + std::cout << "[found] ip=" << e.ip + << " sn=" << e.sn + << " dev_type=" << static_cast(e.dev_type) << "\n"; +} + +static int verb_search(const std::vector& args) { + if (!args.empty() && (args[0] == "--help" || args[0] == "-h")) { + std::cout << kSearchUsage; + return 0; + } + + std::string host_ip; + int timeout_s = 10; + + if (args.size() >= 1) host_ip = args[0]; + if (args.size() >= 2) timeout_s = std::stoi(args[1]); + + if (host_ip.empty()) { + std::cout << "[info] No host IP specified; SDK will auto-detect the NIC.\n"; + } else { + std::cout << "[info] Host NIC: " << host_ip << "\n"; + } + std::cout << "[info] Scanning for " << timeout_s << " second(s)...\n\n"; + + g_search.devices.clear(); + std::string tmp_cfg; + if (!InitSdk(host_ip, OnSearchLidarInfo, nullptr, tmp_cfg)) return 1; + + std::this_thread::sleep_for(std::chrono::seconds(timeout_s)); + + CleanupSdk(tmp_cfg); + + std::lock_guard lock(g_search.mtx); + if (g_search.devices.empty()) { + std::cout << "\n[result] No devices found.\n"; + return 1; + } + + std::cout << "\n[result] " << g_search.devices.size() << " device(s) found:\n"; + std::cout << std::left + << std::setw(18) << "IP" + << std::setw(20) << "Serial Number" + << "Dev Type\n" + << std::string(50, '-') << "\n"; + for (const auto& d : g_search.devices) { + std::cout << std::left + << std::setw(18) << d.ip + << std::setw(20) << d.sn + << static_cast(d.dev_type) << "\n"; + } + return 0; +} + +// --------------------------------------------------------------------------- +// verb: setup (set static IP) +// --------------------------------------------------------------------------- + +static const char* kSetupUsage = R"( +Usage: + livox setup [host_ip] + +Arguments: + current_ip Current IP address of the lidar + new_ip New static IP to assign + netmask Subnet mask (e.g. 255.255.255.0) + gateway Gateway address (e.g. 192.168.1.1) + host_ip [optional] IP of the host NIC (auto-detected if omitted) + +Examples: + livox setup 192.168.1.100 192.168.1.167 255.255.255.0 192.168.1.1 + livox setup 192.168.1.100 192.168.1.167 255.255.255.0 192.168.1.1 192.168.1.10 + +IMPORTANT: Power-cycle the lidar after this command for the new IP to take effect. +)"; + +struct SetupState { + std::string target_ip; + std::string new_ip; + std::string new_netmask; + std::string new_gateway; + + std::atomic done{false}; + std::atomic ok{false}; + + std::mutex mtx; + std::condition_variable cv; +}; + +static SetupState g_setup; + +static void OnSetupIpResult(livox_status status, uint32_t /*handle*/, + LivoxLidarAsyncControlResponse* resp, + void* /*client_data*/) { + if (status == kLivoxLidarStatusSuccess && resp && resp->ret_code == 0) { + std::cout << "[ok] IP updated to " << g_setup.new_ip << "\n"; + g_setup.ok = true; + } else { + int ret = resp ? static_cast(resp->ret_code) : -1; + std::cerr << "[err] IP set failed status=" << status + << " ret_code=" << ret << "\n"; + } + g_setup.done = true; + g_setup.cv.notify_all(); +} + +static void OnSetupLidarInfo(const uint32_t handle, + const LivoxLidarInfo* info, + void* /*client_data*/) { + if (!info) return; + if (std::string(info->lidar_ip) != g_setup.target_ip) return; + + std::cout << "[info] Found target lidar at " << g_setup.target_ip << "\n"; + + LivoxLidarIpInfo cfg{}; + strncpy(cfg.ip_addr, g_setup.new_ip.c_str(), sizeof(cfg.ip_addr) - 1); + strncpy(cfg.net_mask, g_setup.new_netmask.c_str(), sizeof(cfg.net_mask) - 1); + strncpy(cfg.gw_addr, g_setup.new_gateway.c_str(), sizeof(cfg.gw_addr) - 1); + + livox_status rc = SetLivoxLidarIp(handle, &cfg, OnSetupIpResult, nullptr); + if (rc != kLivoxLidarStatusSuccess) { + std::cerr << "[err] SetLivoxLidarIp() failed rc=" << rc << "\n"; + g_setup.done = true; + g_setup.cv.notify_all(); + } +} + +static int verb_setup(const std::vector& args) { + if (args.empty() || args[0] == "--help" || args[0] == "-h") { + std::cout << kSetupUsage; + return 0; + } + if (args.size() < 4) { + std::cerr << "[err] setup requires at least 4 arguments.\n" << kSetupUsage; + return 1; + } + + g_setup.target_ip = args[0]; + g_setup.new_ip = args[1]; + g_setup.new_netmask = args[2]; + g_setup.new_gateway = args[3]; + std::string host_ip = (args.size() >= 5) ? args[4] : ""; + + std::cout << "=== Livox IP setup ===\n" + << " Current IP : " << g_setup.target_ip << "\n" + << " New IP : " << g_setup.new_ip << "\n" + << " Netmask : " << g_setup.new_netmask << "\n" + << " Gateway : " << g_setup.new_gateway << "\n"; + if (!host_ip.empty()) std::cout << " Host NIC : " << host_ip << "\n"; + std::cout << "\n"; + + std::string tmp_cfg; + if (!InitSdk(host_ip, OnSetupLidarInfo, nullptr, tmp_cfg)) return 1; + + constexpr int kTimeoutS = 15; + std::unique_lock lock(g_setup.mtx); + bool completed = g_setup.cv.wait_for(lock, std::chrono::seconds(kTimeoutS), + [] { return g_setup.done.load(); }); + + CleanupSdk(tmp_cfg); + + if (!completed) { + std::cerr << "[err] Timed out after " << kTimeoutS << " s. " + "Lidar not found or not responding.\n"; + return 1; + } + if (!g_setup.ok) { + std::cerr << "[err] IP configuration FAILED.\n"; + return 1; + } + std::cout << "[ok] Done. POWER-CYCLE the lidar for the new IP to take effect.\n"; + return 0; +} + +// --------------------------------------------------------------------------- +// verb: reboot / reset +// --------------------------------------------------------------------------- + +static const char* kRebootUsage = R"( +Usage: + livox reboot [ip2 ...] [--host-ip ] + livox reboot --all [--host-ip ] + + livox reset [ip2 ...] [--host-ip ] + livox reset --all [--host-ip ] + +Options: + --host-ip Host NIC IP (auto-detected if omitted) + --all Target all discovered devices + +NOTE: reboot = firmware restart (~30 s to come back online) + reset = config/state reset (not a full reboot) +)"; + +struct DeviceJob { + std::string ip; + bool triggered{false}; + bool completed{false}; + bool success{false}; +}; + +struct RebootState { + std::set target_ips; + bool target_all{false}; + bool use_reset{false}; + + std::vector jobs; + std::mutex mtx; + std::condition_variable cv; +}; + +static RebootState g_reboot; + +static void OnRebootResponse(livox_status status, uint32_t handle, + LivoxLidarRebootResponse* resp, + void* /*client_data*/) { + std::string ip = HandleToIp(handle); + std::lock_guard lock(g_reboot.mtx); + for (auto& j : g_reboot.jobs) { + if (j.ip == ip && !j.completed) { + j.completed = true; + j.success = (status == kLivoxLidarStatusSuccess && resp && resp->ret_code == 0); + if (j.success) + std::cout << "[ok] " << ip << " reboot command accepted.\n"; + else + std::cerr << "[err] " << ip << " reboot failed" + << " status=" << status + << " ret_code=" << (resp ? (int)resp->ret_code : -1) << "\n"; + break; + } + } + g_reboot.cv.notify_all(); +} + +static void OnResetResponse(livox_status status, uint32_t handle, + LivoxLidarResetResponse* resp, + void* /*client_data*/) { + std::string ip = HandleToIp(handle); + std::lock_guard lock(g_reboot.mtx); + for (auto& j : g_reboot.jobs) { + if (j.ip == ip && !j.completed) { + j.completed = true; + j.success = (status == kLivoxLidarStatusSuccess && resp && resp->ret_code == 0); + if (j.success) + std::cout << "[ok] " << ip << " reset command accepted.\n"; + else + std::cerr << "[err] " << ip << " reset failed" + << " status=" << status + << " ret_code=" << (resp ? (int)resp->ret_code : -1) << "\n"; + break; + } + } + g_reboot.cv.notify_all(); +} + +static void OnRebootLidarInfo(const uint32_t handle, + const LivoxLidarInfo* info, + void* /*client_data*/) { + if (!info) return; + std::string discovered_ip(info->lidar_ip); + + std::lock_guard lock(g_reboot.mtx); + bool is_target = g_reboot.target_all || + g_reboot.target_ips.count(discovered_ip) > 0; + if (!is_target) return; + + DeviceJob* job = nullptr; + for (auto& j : g_reboot.jobs) + if (j.ip == discovered_ip) { job = &j; break; } + if (!job) { + g_reboot.jobs.push_back(DeviceJob{discovered_ip}); + job = &g_reboot.jobs.back(); + } + if (job->triggered) return; + job->triggered = true; + + livox_status rc; + if (g_reboot.use_reset) { + std::cout << "[info] Sending Reset to " << discovered_ip << " ...\n"; + rc = LivoxLidarRequestReset(handle, OnResetResponse, nullptr); + } else { + std::cout << "[info] Sending Reboot to " << discovered_ip << " ...\n"; + rc = LivoxLidarRequestReboot(handle, OnRebootResponse, nullptr); + } + + if (rc != kLivoxLidarStatusSuccess) { + std::cerr << "[err] Command send failed for " << discovered_ip + << " rc=" << rc << "\n"; + job->completed = true; + job->success = false; + g_reboot.cv.notify_all(); + } +} + +static int verb_reboot_or_reset(const std::vector& args, + bool use_reset) { + if (!args.empty() && (args[0] == "--help" || args[0] == "-h")) { + std::cout << kRebootUsage; + return 0; + } + + g_reboot.target_ips.clear(); + g_reboot.jobs.clear(); + g_reboot.target_all = false; + g_reboot.use_reset = use_reset; + + std::string host_ip; + for (std::size_t i = 0; i < args.size(); ++i) { + if (args[i] == "--all") { + g_reboot.target_all = true; + } else if (args[i] == "--host-ip" && i + 1 < args.size()) { + host_ip = args[++i]; + } else if (args[i].rfind("--", 0) == 0) { + std::cerr << "[err] Unknown option: " << args[i] << "\n"; + return 1; + } else { + g_reboot.target_ips.insert(args[i]); + g_reboot.jobs.push_back(DeviceJob{args[i]}); + } + } + + if (!g_reboot.target_all && g_reboot.target_ips.empty()) { + std::cerr << "[err] No target IP addresses specified.\n" << kRebootUsage; + return 1; + } + + const char* cmd = use_reset ? "Reset" : "Reboot"; + std::cout << "=== Livox LiDAR " << cmd << " ===\n"; + if (g_reboot.target_all) + std::cout << " Targets: all discovered devices\n"; + else + for (const auto& ip : g_reboot.target_ips) + std::cout << " Target: " << ip << "\n"; + if (!host_ip.empty()) std::cout << " Host NIC: " << host_ip << "\n"; + std::cout << "\n"; + + std::string tmp_cfg; + if (!InitSdk(host_ip, OnRebootLidarInfo, nullptr, tmp_cfg)) return 1; + + // For --all, sleep a discovery grace period before checking completion. + if (g_reboot.target_all) { + std::this_thread::sleep_for(std::chrono::seconds(5)); + } + + constexpr int kTimeoutS = 15; + auto all_done = [&]() -> bool { + if (g_reboot.jobs.empty()) return false; + for (const auto& j : g_reboot.jobs) + if (!j.completed) return false; + return true; + }; + + int remaining_s = g_reboot.target_all ? (kTimeoutS - 5) : kTimeoutS; + std::unique_lock lock(g_reboot.mtx); + g_reboot.cv.wait_for(lock, std::chrono::seconds(remaining_s), all_done); + + CleanupSdk(tmp_cfg); + + std::cout << "\n=== Summary ===\n"; + int ok_count = 0, fail_count = 0; + for (const auto& j : g_reboot.jobs) { + if (!j.triggered) { + std::cerr << "[err] " << j.ip << " — device not found.\n"; + ++fail_count; + } else if (!j.completed) { + std::cerr << "[err] " << j.ip << " — timed out.\n"; + ++fail_count; + } else if (!j.success) { + ++fail_count; + } else { + ++ok_count; + } + } + if (g_reboot.target_all && g_reboot.jobs.empty()) { + std::cerr << "[err] No devices discovered.\n"; + return 1; + } + std::cout << " " << ok_count << " device(s) successfully commanded.\n"; + if (fail_count > 0) + std::cout << " " << fail_count << " device(s) failed.\n"; + if (!use_reset) + std::cout << "\nNote: allow ~30 s for rebooted device(s) to come back online.\n"; + + return fail_count > 0 ? 1 : 0; +} + +// --------------------------------------------------------------------------- +// verb: firmware_upgrade +// --------------------------------------------------------------------------- + +static const char* kUpgradeUsage = R"( +Usage: + livox firmware_upgrade [ip1 ip2 ...] [--all] [--host-ip ] + +Arguments: + firmware.bin Path to the firmware binary file (.bin) + ip1 ip2 ... Specific lidar IPs to upgrade (omit to use --all) + +Options: + --all Upgrade all discovered devices + --host-ip Host NIC IP (auto-detected if omitted) + +Examples: + livox firmware_upgrade /path/to/MID360_FW_v13.18.0244.bin 192.168.1.167 + livox firmware_upgrade /path/to/firmware.bin --all + livox firmware_upgrade /path/to/firmware.bin 192.168.1.167 192.168.1.184 + +NOTE: The lidar reboots automatically after a successful firmware upgrade. + Do not power off the device during the upgrade process. +)"; + +struct UpgradeJob { + std::string ip; + uint32_t handle{0}; + bool triggered{false}; + bool completed{false}; + bool success{false}; + int progress{0}; +}; + +struct UpgradeState { + std::string firmware_path; + std::set target_ips; + bool target_all{false}; + + std::vector jobs; + std::mutex mtx; + std::condition_variable cv; +}; + +static UpgradeState g_upgrade; + +static void OnUpgradeWorkMode(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse* resp, + void* /*client_data*/) { + if (status != kLivoxLidarStatusSuccess || !resp || resp->ret_code != 0) { + std::cerr << "[warn] SetWorkMode(Upgrade) returned error for handle " + << handle << "\n"; + } +} + +static void OnUpgradeProgress(uint32_t handle, LivoxLidarUpgradeState state, + void* /*client_data*/) { + std::string ip = HandleToIp(handle); + std::lock_guard lock(g_upgrade.mtx); + + for (auto& j : g_upgrade.jobs) { + if (j.handle != handle) continue; + j.progress = state.progress; + + if (state.state == kLivoxLidarUpgradeComplete || state.progress == 100) { + if (!j.completed) { + j.completed = true; + j.success = true; + std::cout << "[ok] " << ip << " firmware upgrade complete.\n"; + g_upgrade.cv.notify_all(); + } + } else if (state.state == kLivoxLidarUpgradeErr || + state.state == kLivoxLidarUpgradeTimeout) { + if (!j.completed) { + j.completed = true; + j.success = false; + std::cerr << "[err] " << ip << " firmware upgrade FAILED (state=" + << state.state << ").\n"; + g_upgrade.cv.notify_all(); + } + } else { + // Print progress bar + std::cout << "\r[info] " << ip << " upgrading... " + << std::setw(3) << state.progress << "%" << std::flush; + } + break; + } +} + +static void OnUpgradeLidarInfo(const uint32_t handle, + const LivoxLidarInfo* info, + void* /*client_data*/) { + if (!info) return; + std::string discovered_ip(info->lidar_ip); + + bool is_target = g_upgrade.target_all || + g_upgrade.target_ips.count(discovered_ip) > 0; + if (!is_target) return; + + std::lock_guard lock(g_upgrade.mtx); + + UpgradeJob* job = nullptr; + for (auto& j : g_upgrade.jobs) + if (j.ip == discovered_ip) { job = &j; break; } + if (!job) { + g_upgrade.jobs.push_back(UpgradeJob{discovered_ip}); + job = &g_upgrade.jobs.back(); + } + if (job->triggered) return; + job->handle = handle; + job->triggered = true; + + std::cout << "[info] Found " << discovered_ip + << " — setting upgrade mode ...\n"; + + // Put the lidar into upgrade mode before calling UpgradeLivoxLidars + SetLivoxLidarWorkMode(handle, kLivoxLidarUpgrade, + OnUpgradeWorkMode, nullptr); +} + +static int verb_firmware_upgrade(const std::vector& args) { + if (args.empty() || args[0] == "--help" || args[0] == "-h") { + std::cout << kUpgradeUsage; + return 0; + } + + g_upgrade.firmware_path.clear(); + g_upgrade.target_ips.clear(); + g_upgrade.jobs.clear(); + g_upgrade.target_all = false; + + std::string host_ip; + + // First positional arg is the firmware file + std::size_t idx = 0; + if (args[idx].rfind("--", 0) != 0) { + g_upgrade.firmware_path = args[idx++]; + } + + for (; idx < args.size(); ++idx) { + if (args[idx] == "--all") { + g_upgrade.target_all = true; + } else if (args[idx] == "--host-ip" && idx + 1 < args.size()) { + host_ip = args[++idx]; + } else if (args[idx].rfind("--", 0) == 0) { + std::cerr << "[err] Unknown option: " << args[idx] << "\n"; + return 1; + } else { + g_upgrade.target_ips.insert(args[idx]); + UpgradeJob j; + j.ip = args[idx]; + g_upgrade.jobs.push_back(j); + } + } + + if (g_upgrade.firmware_path.empty()) { + std::cerr << "[err] No firmware file specified.\n" << kUpgradeUsage; + return 1; + } + if (!g_upgrade.target_all && g_upgrade.target_ips.empty()) { + std::cerr << "[err] No target IP addresses and --all not specified.\n" + << kUpgradeUsage; + return 1; + } + + // Verify the firmware file exists + { + std::ifstream test(g_upgrade.firmware_path); + if (!test.is_open()) { + std::cerr << "[err] Firmware file not found: " + << g_upgrade.firmware_path << "\n"; + return 1; + } + } + + std::cout << "=== Livox Firmware Upgrade ===\n" + << " Firmware : " << g_upgrade.firmware_path << "\n"; + if (g_upgrade.target_all) + std::cout << " Targets : all discovered devices\n"; + else + for (const auto& ip : g_upgrade.target_ips) + std::cout << " Target : " << ip << "\n"; + if (!host_ip.empty()) std::cout << " Host NIC : " << host_ip << "\n"; + std::cout << "\nWARNING: Do NOT power off the device(s) during upgrade!\n\n"; + + std::string tmp_cfg; + if (!InitSdk(host_ip, OnUpgradeLidarInfo, nullptr, tmp_cfg)) return 1; + + if (!SetLivoxLidarUpgradeFirmwarePath(g_upgrade.firmware_path.c_str())) { + std::cerr << "[err] SetLivoxLidarUpgradeFirmwarePath() failed. " + "Check that the firmware file path is valid.\n"; + CleanupSdk(tmp_cfg); + return 1; + } + SetLivoxLidarUpgradeProgressCallback(OnUpgradeProgress, nullptr); + + // Discovery grace period: wait for devices to connect and enter upgrade mode + constexpr int kDiscoveryS = 8; + std::cout << "[info] Waiting " << kDiscoveryS + << " s for device(s) to enter upgrade mode...\n"; + std::this_thread::sleep_for(std::chrono::seconds(kDiscoveryS)); + + // Start the upgrade for all discovered target handles + { + std::lock_guard lock(g_upgrade.mtx); + std::vector handles; + for (const auto& j : g_upgrade.jobs) { + if (j.triggered && j.handle != 0) handles.push_back(j.handle); + } + if (handles.empty()) { + std::cerr << "[err] No target device(s) found after " << kDiscoveryS + << " s. Aborting.\n"; + CleanupSdk(tmp_cfg); + return 1; + } + std::cout << "[info] Starting upgrade for " + << handles.size() << " device(s)...\n"; + UpgradeLivoxLidars(handles.data(), static_cast(handles.size())); + } + + // Wait for all upgrades to finish (max 10 minutes) + constexpr int kUpgradeTimeoutS = 600; + auto all_done = [&]() -> bool { + for (const auto& j : g_upgrade.jobs) + if (j.triggered && !j.completed) return false; + return !g_upgrade.jobs.empty(); + }; + + std::unique_lock lock(g_upgrade.mtx); + bool completed = g_upgrade.cv.wait_for( + lock, std::chrono::seconds(kUpgradeTimeoutS), all_done); + lock.unlock(); + + std::cout << "\n"; // newline after progress bar + CleanupSdk(tmp_cfg); + + std::cout << "\n=== Upgrade Summary ===\n"; + int ok_count = 0, fail_count = 0; + for (const auto& j : g_upgrade.jobs) { + if (!j.triggered) { + std::cerr << "[err] " << j.ip << " — device not found.\n"; + ++fail_count; + } else if (!j.completed) { + std::cerr << "[err] " << j.ip << " — timed out.\n"; + ++fail_count; + } else if (!j.success) { + ++fail_count; + } else { + ++ok_count; + } + } + if (!completed) { + std::cerr << "[err] Upgrade timed out after " << kUpgradeTimeoutS << " s.\n"; + } + std::cout << " " << ok_count << " device(s) upgraded successfully.\n"; + if (fail_count > 0) + std::cout << " " << fail_count << " device(s) failed.\n"; + + return (fail_count > 0 || !completed) ? 1 : 0; +} + +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- + +static const char* kTopLevelUsage = R"( +livox — Unified Livox LiDAR command-line tool + +Usage: + livox [options] + +Verbs: + search Discover all Livox devices on the network + setup Assign a static IP address to a lidar + reboot Soft-reboot one or more lidars (firmware restart) + reset Reset one or more lidars (config/state reset) + firmware_upgrade Upgrade the firmware on one or more lidars + +Run livox --help for per-verb usage. +)"; + +int main(int argc, char** argv) { + if (argc < 2) { + std::cout << kTopLevelUsage; + return 1; + } + + std::string verb(argv[1]); + std::vector args; + for (int i = 2; i < argc; ++i) args.emplace_back(argv[i]); + + if (verb == "search") return verb_search(args); + if (verb == "setup") return verb_setup(args); + if (verb == "reboot") return verb_reboot_or_reset(args, false); + if (verb == "reset") return verb_reboot_or_reset(args, true); + if (verb == "firmware_upgrade") return verb_firmware_upgrade(args); + + std::cerr << "[err] Unknown verb: " << verb << "\n" << kTopLevelUsage; + return 1; +}