Livox 3D lidar support

Dockerfile

@@ -528,6 +528,25 @@ RUN wget -qO - https://github.com/borglab/gtsam/archive/refs/tags/${GTSAM_VERSIO
     && make -j$(nproc) install \
     && rm -rf /tmp/*
 
+### INSTALL LIVOX SDK2
+
+ARG LIVOX_VERSION="1.2.5"
+
+RUN wget -qO - https://github.com/Livox-SDK/Livox-SDK2/archive/refs/tags/v${LIVOX_VERSION}.tar.gz | tar -xz \
+    && cd Livox-SDK2-${LIVOX_VERSION} && mkdir build && cd build \
+    && cmake .. && make -j$(nproc) && make install \
+    && rm -rf /tmp/*
+
+### INSTALL RAPIDJSON
+
+ARG RAPIDJSON_VERSION="1.1.0"
+
+RUN wget -qO - https://github.com/Tencent/rapidjson/archive/refs/tags/v${RAPIDJSON_VERSION}.tar.gz | tar -xz \
+    && cd rapidjson-${RAPIDJSON_VERSION} && mkdir build && cd build \
+    && cmake .. -DRAPIDJSON_BUILD_DOC=OFF -DRAPIDJSON_BUILD_EXAMPLES=OFF -DRAPIDJSON_BUILD_TESTS=OFF \
+    && make -j$(nproc) && make install \
+    && rm -rf /tmp/*
+
 ### INSTALL DEV PKGS
 
 COPY requirements.txt /tmp/requirements.txt

lidar-net.yaml

@@ -0,0 +1,20 @@
+# Use together with main docker-compose.yaml file:
+# docker compose -f docker-compose.yaml -f lidar-net.yaml up -d
+
+services:
+  truck:
+    networks:
+      default:
+      lidar-net:
+
+networks:
+  lidar-net:
+    name: "${CONTAINER_NAME:-truck-${USER}}-lidar-net"
+    driver: macvlan
+    driver_opts:
+      parent: eth0
+    ipam:
+      config:
+        - subnet: 192.168.2.0/24
+          gateway: 192.168.2.1
+          ip_range: "192.168.2.1${PORT_SUFFIX:-00}/32"

packages/livox_driver/CMakeLists.txt

@@ -0,0 +1,131 @@
+# Copyright(c) 2020 livoxtech limited.
+
+cmake_minimum_required(VERSION 3.14)
+project(livox_driver)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
+endif()
+
+# Printf version info
+include(cmake/version.cmake)
+project(
+  ${PROJECT_NAME}
+  VERSION ${LIVOX_ROS_DRIVER2_VERSION}
+  LANGUAGES CXX
+)
+message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
+
+# -----------------------------------------------------------------------------
+# Add ROS Version MACRO
+# -----------------------------------------------------------------------------
+
+# find dependencies uncomment the following section in order to fill in further
+# dependencies manually. find_package(<dependency> REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(RapidJSON)
+
+# generate custom msg headers
+set(LIVOX_INTERFACES livox_interfaces2)
+rosidl_generate_interfaces(
+  ${LIVOX_INTERFACES}
+  "msg/CustomPoint.msg"
+  "msg/CustomMsg.msg"
+  DEPENDENCIES
+  builtin_interfaces
+  std_msgs
+  LIBRARY_NAME
+  ${PROJECT_NAME}
+)
+
+# make sure the livox_lidar_sdk_shared library is installed
+find_library(
+  LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED
+)
+
+#
+find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR NAMES "livox_lidar_api.h"
+                                            "livox_lidar_def.h" REQUIRED
+)
+
+# livox ros2 driver target
+ament_auto_add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/livox_driver.cpp
+  src/lddc.cpp
+  src/driver_node.cpp
+  src/lds.cpp
+  src/lds_lidar.cpp
+  src/comm/comm.cpp
+  src/comm/ldq.cpp
+  src/comm/semaphore.cpp
+  src/comm/lidar_imu_data_queue.cpp
+  src/comm/cache_index.cpp
+  src/comm/pub_handler.cpp
+  src/parse_cfg_file/parse_cfg_file.cpp
+  src/parse_cfg_file/parse_livox_lidar_cfg.cpp
+  src/call_back/lidar_common_callback.cpp
+  src/call_back/livox_lidar_callback.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS})
+
+# get include directories of custom msg headers if(HUMBLE_ROS STREQUAL "humble")
+
+rosidl_get_typesupport_target(
+  cpp_typesupport_target ${LIVOX_INTERFACES} "rosidl_typesupport_cpp"
+)
+target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
+
+# else() set(LIVOX_INTERFACE_TARGET
+# "${LIVOX_INTERFACES}__rosidl_typesupport_cpp")
+# add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES})
+# get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES
+# ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES) endif()
+
+# include file direcotry
+target_include_directories(
+  ${PROJECT_NAME}
+  PUBLIC ${LIVOX_LIDAR_SDK_INCLUDE_DIR}
+         ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs
+         src
+)
+
+# link libraries
+target_link_libraries(
+  ${PROJECT_NAME} ${LIVOX_LIDAR_SDK_LIBRARY}
+  ${LIVOX_INTERFACE_TARGET} # for custom msgs
+  ${PPT_LIBRARY} ${Boost_LIBRARY}
+)
+
+rclcpp_components_register_node(
+  ${PROJECT_NAME} PLUGIN "livox_ros::DriverNode" EXECUTABLE
+  ${PROJECT_NAME}_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights uncomment
+  # the line when a copyright and license is not present in all source files
+  # set(ament_cmake_copyright_FOUND TRUE) the following line skips cpplint (only
+  # works in a git repo) uncomment the line when this package is not in a git
+  # repo set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE launch)

packages/livox_driver/cmake/version.cmake

@@ -0,0 +1,30 @@
+# -----------------------------------------------------------------------------
+# Get livox_driver version from include/livox_driver.h
+# -----------------------------------------------------------------------------
+file(READ "${CMAKE_CURRENT_LIST_DIR}/../src/include/livox_driver.h"
+     LIVOX_ROS_DRIVER2_VERSION_FILE
+)
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MAJOR ([0-9]+)" _
+             "${LIVOX_ROS_DRIVER2_VERSION_FILE}"
+)
+set(VER_MAJOR ${CMAKE_MATCH_1})
+
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MINOR ([0-9]+)" _
+             "${LIVOX_ROS_DRIVER2_VERSION_FILE}"
+)
+set(VER_MINOR ${CMAKE_MATCH_1})
+
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_PATCH ([0-9]+)" _
+             "${LIVOX_ROS_DRIVER2_VERSION_FILE}"
+)
+set(VER_PATCH ${CMAKE_MATCH_1})
+
+if(NOT DEFINED VER_MAJOR
+   OR NOT DEFINED VER_MINOR
+   OR NOT DEFINED VER_PATCH
+)
+  message(
+    FATAL_ERROR "Could not extract valid version from include/livox_driver.h"
+  )
+endif()
+set(LIVOX_ROS_DRIVER2_VERSION "${VER_MAJOR}.${VER_MINOR}.${VER_PATCH}")

packages/livox_driver/launch/livox_driver.py

@@ -0,0 +1,132 @@
+import copy
+import json
+import socket
+
+import psutil
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+LAUNCH_PARAMS = {
+    "lidar_ip": "192.168.2.10",
+    "host_iface": "eth1",
+    "frame_id": "livox",
+    "publish_freq": "10",
+    "scan_pattern_mode": "0",
+}
+
+NODE_PARAMS = {
+    "frame_id": ...,  # from launch params
+    "publish_freq": ...,  # from launch params
+    "user_config_path": ...,  # generated in runtime
+    "xfer_format": 0,  # 0: Pointcloud2(PointXYZRTL), 1: Custom format
+    "multi_topic": 0,  # 0: all lidars same topic, 1: one lidar one topic
+    "data_src": 0,  # keep default
+    "output_data_type": 0,  # keep default
+}
+
+# https://github.com/Livox-SDK/livox_ros_driver2?tab=readme-ov-file#4-lidar-config
+# pattern_mode (from launch params):
+#   0: non-repeating scanning pattern mode
+#   1: repeating scanning pattern mode
+#   2: repeating scanning pattern mode (low scanning rate)
+# pcl_data_type:
+#   1: Cartesian coordinate data (32 bits)
+#   2: Cartesian coordinate data (16 bits)
+#   3: Spherical coordinate data
+# do not touch:
+#   host_ip: detected automatically
+#   lidar_configs > ip: taken from launch params
+#   lidar_type: 8 (no other options)
+#   *_data_port: keep defaults
+#   extrinsic_parameter: keep zeros because we're using transforms
+LIVOX_CONFIG = {
+    "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": {
+            "host_ip": ...,
+            "cmd_data_port": 56101,
+            "push_msg_port": 56201,
+            "point_data_port": 56301,
+            "imu_data_port": 56401,
+            "log_data_port": 56501,
+        },
+    },
+    "lidar_configs": [
+        {
+            "ip": ...,
+            "pattern_mode": ...,
+            "pcl_data_type": 1,
+            "extrinsic_parameter": {
+                "roll": 0.0,
+                "pitch": 0.0,
+                "yaw": 0.0,
+                "x": 0,
+                "y": 0,
+                "z": 0,
+            },
+        }
+    ],
+}
+
+
+# https://stackoverflow.com/a/73559817
+def get_ip_address(interface: str) -> str:
+    interface_addrs = psutil.net_if_addrs().get(interface) or []
+    for snicaddr in interface_addrs:
+        if snicaddr.family == socket.AF_INET:
+            return snicaddr.address
+
+
+# https://robotics.stackexchange.com/a/104402
+def launch_setup(context):
+    launch_params = {x: LaunchConfiguration(x).perform(context) for x in LAUNCH_PARAMS}
+    lidar_ip = launch_params["lidar_ip"]
+    host_iface = launch_params["host_iface"]
+    host_ip = get_ip_address("eth1")
+    assert host_ip is not None, f"Failed to get host IP for interface {host_iface}"
+
+    config = copy.deepcopy(LIVOX_CONFIG)
+    config["MID360"]["host_net_info"]["host_ip"] = host_ip
+    config["lidar_configs"][0]["ip"] = lidar_ip
+    config["lidar_configs"][0]["pattern_mode"] = int(launch_params["scan_pattern_mode"])
+    config_path = "/tmp/livox-config.json"
+    with open(config_path, "w") as file:
+        json.dump(config, file, sort_keys=False)
+
+    print(f"[livox launch] Lidar IP: {lidar_ip}")
+    print(f"[livox launch] Host IP: {host_ip} ({host_iface})")
+    print(f"[livox launch] Config path: {config_path}")
+
+    node_params = copy.deepcopy(NODE_PARAMS)
+    node_params["frame_id"] = launch_params["frame_id"]
+    node_params["publish_freq"] = float(launch_params["publish_freq"])
+    node_params["user_config_path"] = config_path
+    node_params = [{name: value} for name, value in node_params.items()]
+
+    node = Node(
+        package="livox_driver",
+        name="livox_driver",
+        executable="livox_driver_node",
+        output="screen",
+        parameters=node_params,
+    )
+    return [node]
+
+
+def generate_launch_description():
+    launch_params = [
+        DeclareLaunchArgument(name, default_value=value)
+        for name, value in LAUNCH_PARAMS.items()
+    ]
+    description = LaunchDescription(launch_params)
+    description.add_action(OpaqueFunction(function=launch_setup))
+    return description

packages/livox_driver/msg/CustomMsg.msg

@@ -0,0 +1,8 @@
+# Livox publish pointcloud msg format.
+
+std_msgs/Header header    # ROS standard message header
+uint64 timebase           # The time of first point
+uint32 point_num          # Total number of pointclouds
+uint8  lidar_id           # Lidar device id number
+uint8[3]  rsvd            # Reserved use
+CustomPoint[] points      # Pointcloud data

packages/livox_driver/msg/CustomPoint.msg

@@ -0,0 +1,9 @@
+# Livox costom pointcloud format.
+
+uint32 offset_time      # offset time relative to the base time
+float32 x               # X axis, unit:m
+float32 y               # Y axis, unit:m
+float32 z               # Z axis, unit:m
+uint8 reflectivity      # reflectivity, 0~255
+uint8 tag               # livox tag
+uint8 line              # laser number in lidar

packages/livox_driver/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>livox_driver</name>
+  <version>1.0.0</version>
+  <description>The ROS device driver for Livox 3D LiDARs, for ROS2</description>
+  <maintainer email="[email protected]">feng</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rcutils</depend>
+  <depend>rcl_interfaces</depend>
+
+  <exec_depend>rosbag2</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>