From 672062193866fd58ffd0ee8fb000d4cfada38097 Mon Sep 17 00:00:00 2001 From: AndBondStyle Date: Sun, 3 Nov 2024 00:20:49 +0300 Subject: [PATCH 01/10] Update dockerfile, add livox node + initial fixes --- Dockerfile | 9 + packages/livox_driver/CMakeLists.txt | 151 ++++ packages/livox_driver/cmake/version.cmake | 30 + .../livox_driver/config/MID360_config.json | 41 + .../livox_driver/launch/msg_MID360_launch.py | 55 ++ packages/livox_driver/msg/CustomMsg.msg | 8 + packages/livox_driver/msg/CustomPoint.msg | 9 + packages/livox_driver/package.xml | 35 + .../src/call_back/lidar_common_callback.cpp | 70 ++ .../src/call_back/lidar_common_callback.h | 40 + .../src/call_back/livox_lidar_callback.cpp | 353 +++++++++ .../src/call_back/livox_lidar_callback.h | 69 ++ .../livox_driver/src/comm/cache_index.cpp | 129 ++++ packages/livox_driver/src/comm/cache_index.h | 55 ++ packages/livox_driver/src/comm/comm.cpp | 69 ++ packages/livox_driver/src/comm/comm.h | 303 ++++++++ packages/livox_driver/src/comm/ldq.cpp | 146 ++++ packages/livox_driver/src/comm/ldq.h | 64 ++ .../src/comm/lidar_imu_data_queue.cpp | 70 ++ .../src/comm/lidar_imu_data_queue.h | 76 ++ .../livox_driver/src/comm/pub_handler.cpp | 450 +++++++++++ packages/livox_driver/src/comm/pub_handler.h | 132 ++++ packages/livox_driver/src/comm/semaphore.cpp | 41 + packages/livox_driver/src/comm/semaphore.h | 48 ++ packages/livox_driver/src/driver_node.cpp | 39 + packages/livox_driver/src/driver_node.h | 78 ++ .../livox_driver/src/include/livox_driver.h | 39 + .../livox_driver/src/include/ros1_headers.h | 53 ++ .../livox_driver/src/include/ros2_headers.h | 52 ++ .../livox_driver/src/include/ros_headers.h | 34 + packages/livox_driver/src/lddc.cpp | 718 ++++++++++++++++++ packages/livox_driver/src/lddc.h | 167 ++++ packages/livox_driver/src/lds.cpp | 206 +++++ packages/livox_driver/src/lds.h | 85 +++ packages/livox_driver/src/lds_lidar.cpp | 211 +++++ packages/livox_driver/src/lds_lidar.h | 96 +++ packages/livox_driver/src/livox_driver.cpp | 222 ++++++ .../src/parse_cfg_file/parse_cfg_file.cpp | 66 ++ .../src/parse_cfg_file/parse_cfg_file.h | 52 ++ .../parse_cfg_file/parse_livox_lidar_cfg.cpp | 153 ++++ .../parse_cfg_file/parse_livox_lidar_cfg.h | 58 ++ scripts/rosenv.sh | 2 +- 42 files changed, 4783 insertions(+), 1 deletion(-) create mode 100644 packages/livox_driver/CMakeLists.txt create mode 100644 packages/livox_driver/cmake/version.cmake create mode 100644 packages/livox_driver/config/MID360_config.json create mode 100644 packages/livox_driver/launch/msg_MID360_launch.py create mode 100644 packages/livox_driver/msg/CustomMsg.msg create mode 100644 packages/livox_driver/msg/CustomPoint.msg create mode 100644 packages/livox_driver/package.xml create mode 100644 packages/livox_driver/src/call_back/lidar_common_callback.cpp create mode 100644 packages/livox_driver/src/call_back/lidar_common_callback.h create mode 100644 packages/livox_driver/src/call_back/livox_lidar_callback.cpp create mode 100644 packages/livox_driver/src/call_back/livox_lidar_callback.h create mode 100644 packages/livox_driver/src/comm/cache_index.cpp create mode 100644 packages/livox_driver/src/comm/cache_index.h create mode 100644 packages/livox_driver/src/comm/comm.cpp create mode 100644 packages/livox_driver/src/comm/comm.h create mode 100644 packages/livox_driver/src/comm/ldq.cpp create mode 100644 packages/livox_driver/src/comm/ldq.h create mode 100644 packages/livox_driver/src/comm/lidar_imu_data_queue.cpp create mode 100644 packages/livox_driver/src/comm/lidar_imu_data_queue.h create mode 100644 packages/livox_driver/src/comm/pub_handler.cpp create mode 100644 packages/livox_driver/src/comm/pub_handler.h create mode 100644 packages/livox_driver/src/comm/semaphore.cpp create mode 100644 packages/livox_driver/src/comm/semaphore.h create mode 100644 packages/livox_driver/src/driver_node.cpp create mode 100644 packages/livox_driver/src/driver_node.h create mode 100644 packages/livox_driver/src/include/livox_driver.h create mode 100644 packages/livox_driver/src/include/ros1_headers.h create mode 100644 packages/livox_driver/src/include/ros2_headers.h create mode 100644 packages/livox_driver/src/include/ros_headers.h create mode 100644 packages/livox_driver/src/lddc.cpp create mode 100644 packages/livox_driver/src/lddc.h create mode 100644 packages/livox_driver/src/lds.cpp create mode 100644 packages/livox_driver/src/lds.h create mode 100644 packages/livox_driver/src/lds_lidar.cpp create mode 100644 packages/livox_driver/src/lds_lidar.h create mode 100644 packages/livox_driver/src/livox_driver.cpp create mode 100644 packages/livox_driver/src/parse_cfg_file/parse_cfg_file.cpp create mode 100644 packages/livox_driver/src/parse_cfg_file/parse_cfg_file.h create mode 100644 packages/livox_driver/src/parse_cfg_file/parse_livox_lidar_cfg.cpp create mode 100644 packages/livox_driver/src/parse_cfg_file/parse_livox_lidar_cfg.h diff --git a/Dockerfile b/Dockerfile index dd7e4a07d..61af1d8a3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -528,6 +528,15 @@ 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 DEV PKGS COPY requirements.txt /tmp/requirements.txt diff --git a/packages/livox_driver/CMakeLists.txt b/packages/livox_driver/CMakeLists.txt new file mode 100644 index 000000000..b07297238 --- /dev/null +++ b/packages/livox_driver/CMakeLists.txt @@ -0,0 +1,151 @@ +# 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() + +list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules") + +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 +# ----------------------------------------------------------------------------- +add_definitions(-DBUILDING_ROS2) + +# find dependencies uncomment the following section in order to fill in further +# dependencies manually. find_package( REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() +find_package(PCL REQUIRED) +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(RapidJSON) + +# check apr +find_package(PkgConfig) +pkg_check_modules(APR apr-1) +if(APR_FOUND) + message(${APR_INCLUDE_DIRS}) + message(${APR_LIBRARIES}) +endif(APR_FOUND) + +# 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 +) + +# PCL library +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +# 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 ${PCL_INCLUDE_DIRS} ${APR_INCLUDE_DIRS} ${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} + ${PCL_LIBRARIES} + ${APR_LIBRARIES} +) + +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 config launch) diff --git a/packages/livox_driver/cmake/version.cmake b/packages/livox_driver/cmake/version.cmake new file mode 100644 index 000000000..5b48ae967 --- /dev/null +++ b/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}") diff --git a/packages/livox_driver/config/MID360_config.json b/packages/livox_driver/config/MID360_config.json new file mode 100644 index 000000000..ad691f4c7 --- /dev/null +++ b/packages/livox_driver/config/MID360_config.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.5", + "cmd_data_port": 56101, + "push_msg_ip": "192.168.1.5", + "push_msg_port": 56201, + "point_data_ip": "192.168.1.5", + "point_data_port": 56301, + "imu_data_ip" : "192.168.1.5", + "imu_data_port": 56401, + "log_data_ip" : "", + "log_data_port": 56501 + } + }, + "lidar_configs" : [ + { + "ip" : "192.168.1.12", + "pcl_data_type" : 1, + "pattern_mode" : 0, + "extrinsic_parameter" : { + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 + } + } + ] +} diff --git a/packages/livox_driver/launch/msg_MID360_launch.py b/packages/livox_driver/launch/msg_MID360_launch.py new file mode 100644 index 000000000..b3d3e89cc --- /dev/null +++ b/packages/livox_driver/launch/msg_MID360_launch.py @@ -0,0 +1,55 @@ +import os + +from launch import LaunchDescription +from launch_ros.actions import Node + +################### user configure parameters for ros2 start ################### +xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format +multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic +data_src = 0 # 0-lidar, others-Invalid data src +publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. +output_type = 0 +frame_id = "livox_frame" +lvx_file_path = "/home/livox/livox_test.lvx" +cmdline_bd_code = "livox0000000001" + +cur_path = os.path.split(os.path.realpath(__file__))[0] + "/" +cur_config_path = cur_path + "../config" +user_config_path = os.path.join(cur_config_path, "MID360_config.json") +################### user configure parameters for ros2 end ##################### + +livox_ros2_params = [ + {"xfer_format": xfer_format}, + {"multi_topic": multi_topic}, + {"data_src": data_src}, + {"publish_freq": publish_freq}, + {"output_data_type": output_type}, + {"frame_id": frame_id}, + {"lvx_file_path": lvx_file_path}, + {"user_config_path": user_config_path}, + {"cmdline_input_bd_code": cmdline_bd_code}, +] + + +def generate_launch_description(): + livox_driver = Node( + package="livox_driver", + executable="livox_driver_node", + name="livox_lidar_publisher", + output="screen", + parameters=livox_ros2_params, + ) + + return LaunchDescription( + [ + livox_driver, + # launch.actions.RegisterEventHandler( + # event_handler=launch.event_handlers.OnProcessExit( + # target_action=livox_rviz, + # on_exit=[ + # launch.actions.EmitEvent(event=launch.events.Shutdown()), + # ] + # ) + # ) + ] + ) diff --git a/packages/livox_driver/msg/CustomMsg.msg b/packages/livox_driver/msg/CustomMsg.msg new file mode 100644 index 000000000..58dd2a3d1 --- /dev/null +++ b/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 diff --git a/packages/livox_driver/msg/CustomPoint.msg b/packages/livox_driver/msg/CustomPoint.msg new file mode 100644 index 000000000..2ee325bcb --- /dev/null +++ b/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 diff --git a/packages/livox_driver/package.xml b/packages/livox_driver/package.xml new file mode 100644 index 000000000..9c3d7b525 --- /dev/null +++ b/packages/livox_driver/package.xml @@ -0,0 +1,35 @@ + + + + livox_driver + 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/packages/livox_driver/src/call_back/lidar_common_callback.cpp b/packages/livox_driver/src/call_back/lidar_common_callback.cpp new file mode 100644 index 000000000..87dcf1daa --- /dev/null +++ b/packages/livox_driver/src/call_back/lidar_common_callback.cpp @@ -0,0 +1,70 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lidar_common_callback.h" + +#include "../lds_lidar.h" + +#include + +namespace livox_ros { + +void LidarCommonCallback::OnLidarPointClounCb(PointFrame* frame, void* client_data) { + if (frame == nullptr) { + printf("LidarPointCloudCb frame is nullptr.\n"); + return; + } + + if (client_data == nullptr) { + printf("Lidar point cloud cb failed, client data is nullptr.\n"); + return; + } + + if (frame->lidar_num == 0) { + printf("LidarPointCloudCb lidar_num:%u.\n", frame->lidar_num); + 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"); + return; + } + if (client_data == nullptr) { + printf("Lidar point cloud cb failed, client data is nullptr.\n"); + return; + } + + LdsLidar* lds_lidar = static_cast(client_data); + lds_lidar->StorageImuData(imu_data); +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/call_back/lidar_common_callback.h b/packages/livox_driver/src/call_back/lidar_common_callback.h new file mode 100644 index 000000000..fbceac044 --- /dev/null +++ b/packages/livox_driver/src/call_back/lidar_common_callback.h @@ -0,0 +1,40 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_ +#define LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_ + +#include "comm/comm.h" + +namespace livox_ros { + +class LidarCommonCallback { + public: + static void OnLidarPointClounCb(PointFrame* frame, void* client_data); + static void LidarImuDataCallback(ImuData* imu_data, void* client_data); +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_ diff --git a/packages/livox_driver/src/call_back/livox_lidar_callback.cpp b/packages/livox_driver/src/call_back/livox_lidar_callback.cpp new file mode 100644 index 000000000..6b0839ff3 --- /dev/null +++ b/packages/livox_driver/src/call_back/livox_lidar_callback.cpp @@ -0,0 +1,353 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "livox_lidar_callback.h" + +#include +#include +#include + +namespace livox_ros { + +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; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + 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; + } + LidarDevice* p_lidar = &(lds_lidar->lidars_[index]); + p_lidar->lidar_type = kLivoxLidarType; + } else { + // 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_); + if (config.pcl_data_type != -1) { + lidar_device->livox_config.set_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; + } + if (config.pattern_mode != -1) { + lidar_device->livox_config.set_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; + } + if (config.blind_spot_set != -1) { + lidar_device->livox_config.set_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; + } + if (config.dual_emit_en != -1) { + lidar_device->livox_config.set_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; + } + } // free lock for set_bits + + // set extrinsic params into lidar + LivoxLidarInstallAttitude attitude{ + config.extrinsic_param.roll, + config.extrinsic_param.pitch, + config.extrinsic_param.yaw, + config.extrinsic_param.x, + config.extrinsic_param.y, + config.extrinsic_param.z}; + SetLivoxLidarInstallAttitude( + config.handle, &attitude, LivoxLidarCallback::SetAttitudeCallback, lds_lidar); + } + + std::cout << "begin to change work mode to 'Normal', handle: " << handle << std::endl; + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr); + EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar); + return; +} + +void LivoxLidarCallback::WorkModeChangedCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* client_data) { + if (status != kLivoxLidarStatusSuccess) { + std::cout << "failed to change work mode, handle: " << handle << ", try again..." + << std::endl; + 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; + return; +} + +void LivoxLidarCallback::SetDataTypeCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* 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; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + if (status == kLivoxLidarStatusSuccess) { + std::lock_guard lock(lds_lidar->config_mutex_); + lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDataType)); + if (!lidar_device->livox_config.set_bits) { + lidar_device->connect_state = kConnectStateSampling; + } + 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; + } else { + std::cout << "failed to set data type, handle: " << handle + << ", return code: " << response->ret_code + << ", error key: " << response->error_key << std::endl; + } + return; +} + +void LivoxLidarCallback::SetPatternModeCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* 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; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + if (status == kLivoxLidarStatusSuccess) { + std::lock_guard lock(lds_lidar->config_mutex_); + lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigScanPattern)); + if (!lidar_device->livox_config.set_bits) { + lidar_device->connect_state = kConnectStateSampling; + } + 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; + } else { + std::cout << "failed to set pattern mode, handle: " << handle + << ", return code: " << response->ret_code + << ", error key: " << response->error_key << std::endl; + } + return; +} + +void LivoxLidarCallback::SetBlindSpotCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* 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; + return; + } + LdsLidar* lds_lidar = static_cast(client_data); + + if (status == kLivoxLidarStatusSuccess) { + std::lock_guard lock(lds_lidar->config_mutex_); + lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigBlindSpot)); + if (!lidar_device->livox_config.set_bits) { + lidar_device->connect_state = kConnectStateSampling; + } + 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; + } else { + std::cout << "failed to set blind spot, handle: " << handle + << ", return code: " << response->ret_code + << ", error key: " << response->error_key << std::endl; + } + return; +} + +void LivoxLidarCallback::SetDualEmitCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* 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; + return; + } + + LdsLidar* lds_lidar = static_cast(client_data); + if (status == kLivoxLidarStatusSuccess) { + std::lock_guard lock(lds_lidar->config_mutex_); + lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDualEmit)); + if (!lidar_device->livox_config.set_bits) { + lidar_device->connect_state = kConnectStateSampling; + } + 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; + } else { + std::cout << "failed to set dual emit mode, handle: " << handle + << ", return code: " << response->ret_code + << ", error key: " << response->error_key << std::endl; + } + return; +} + +void LivoxLidarCallback::SetAttitudeCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* 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; + return; + } + + LdsLidar* lds_lidar = static_cast(client_data); + if (status == kLivoxLidarStatusSuccess) { + std::cout << "successfully set lidar attitude, ip: " << IpNumToString(handle) << std::endl; + } else if (status == kLivoxLidarStatusTimeout) { + std::cout << "set lidar attitude timeout, ip: " << IpNumToString(handle) << ", try again..." + << std::endl; + const UserLivoxLidarConfig& config = lidar_device->livox_config; + LivoxLidarInstallAttitude attitude{ + config.extrinsic_param.roll, + config.extrinsic_param.pitch, + config.extrinsic_param.yaw, + config.extrinsic_param.x, + config.extrinsic_param.y, + config.extrinsic_param.z}; + SetLivoxLidarInstallAttitude( + config.handle, &attitude, LivoxLidarCallback::SetAttitudeCallback, lds_lidar); + } else { + std::cout << "failed to set lidar attitude, ip: " << IpNumToString(handle) << std::endl; + } +} + +void LivoxLidarCallback::EnableLivoxLidarImuDataCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* 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; + 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; + return; + } + + if (status == kLivoxLidarStatusSuccess) { + std::cout << "successfully enable Livox Lidar imu, ip: " << IpNumToString(handle) + << std::endl; + } else if (status == kLivoxLidarStatusTimeout) { + std::cout << "enable Livox Lidar imu timeout, ip: " << IpNumToString(handle) + << ", try again..." << std::endl; + EnableLivoxLidarImuData( + handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar); + } else { + std::cout << "failed to enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl; + } +} + +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; + return nullptr; + } + + LdsLidar* lds_lidar = static_cast(client_data); + uint8_t index = 0; + int8_t ret = lds_lidar->cache_index_.GetIndex(kLivoxLidarType, handle, index); + if (ret != 0) { + return nullptr; + } + + return &(lds_lidar->lidars_[index]); +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/call_back/livox_lidar_callback.h b/packages/livox_driver/src/call_back/livox_lidar_callback.h new file mode 100644 index 000000000..84fa12fbd --- /dev/null +++ b/packages/livox_driver/src/call_back/livox_lidar_callback.h @@ -0,0 +1,69 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_ +#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_ + +#include "../lds.h" +#include "../lds_lidar.h" +#include "../comm/comm.h" + +#include "livox_lidar_api.h" +#include "livox_lidar_def.h" + +namespace livox_ros { + +class LivoxLidarCallback { + public: + static void LidarInfoChangeCallback( + const uint32_t handle, const LivoxLidarInfo* info, void* client_data); + static void WorkModeChangedCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* client_data); + static void SetDataTypeCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* client_data); + static void SetPatternModeCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* client_data); + static void SetBlindSpotCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* client_data); + static void SetDualEmitCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* client_data); + static void SetAttitudeCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* client_data); + static void EnableLivoxLidarImuDataCallback( + livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse* response, + void* client_data); + + private: + static LidarDevice* GetLidarDevice(const uint32_t handle, void* client_data); +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_ diff --git a/packages/livox_driver/src/comm/cache_index.cpp b/packages/livox_driver/src/comm/cache_index.cpp new file mode 100644 index 000000000..386f275e0 --- /dev/null +++ b/packages/livox_driver/src/comm/cache_index.cpp @@ -0,0 +1,129 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "cache_index.h" +#include "livox_lidar_def.h" + +namespace livox_ros { + +CacheIndex::CacheIndex() { + std::array index_cache = {0}; + index_cache_.swap(index_cache); +} + +int8_t CacheIndex::GetFreeIndex( + const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) { + std::string key; + int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key); + if (ret != 0) { + return -1; + } + { + std::lock_guard lock(index_mutex_); + if (map_index_.find(key) != map_index_.end()) { + index = map_index_[key]; + return 0; + } + } + + { + printf("GetFreeIndex key:%s.\n", key.c_str()); + std::lock_guard lock(index_mutex_); + for (size_t i = 0; i < kMaxSourceLidar; ++i) { + if (!index_cache_[i]) { + index_cache_[i] = 1; + map_index_[key] = static_cast(i); + index = static_cast(i); + return 0; + } + } + } + return -1; +} + +int8_t CacheIndex::GenerateIndexKey( + const uint8_t livox_lidar_type, const uint32_t handle, std::string& key) { + if (livox_lidar_type == kLivoxLidarType) { + key = "livox_lidar_" + std::to_string(handle); + } else { + printf( + "Can not generate index, the livox lidar type is unknown, the livox lidar type:%u\n", + livox_lidar_type); + return -1; + } + return 0; +} + +int8_t CacheIndex::GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) { + std::string key; + int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key); + if (ret != 0) { + return -1; + } + + if (map_index_.find(key) != map_index_.end()) { + std::lock_guard lock(index_mutex_); + index = map_index_[key]; + return 0; + } + printf("Can not get index, the livox lidar type:%u, handle:%u\n", livox_lidar_type, handle); + return -1; +} + +int8_t CacheIndex::LvxGetIndex( + const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) { + std::string key; + int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key); + if (ret != 0) { + return -1; + } + + if (map_index_.find(key) != map_index_.end()) { + index = map_index_[key]; + return 0; + } + + return GetFreeIndex(livox_lidar_type, handle, index); +} + +void CacheIndex::ResetIndex(LidarDevice* lidar) { + std::string key; + int8_t ret = GenerateIndexKey(lidar->lidar_type, lidar->handle, key); + if (ret != 0) { + printf( + "Reset index failed, can not generate index key, lidar type:%u, handle:%u.\n", + lidar->lidar_type, + lidar->handle); + return; + } + + if (map_index_.find(key) != map_index_.end()) { + uint8_t index = map_index_[key]; + std::lock_guard lock(index_mutex_); + map_index_.erase(key); + index_cache_[index] = 0; + } +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/comm/cache_index.h b/packages/livox_driver/src/comm/cache_index.h new file mode 100644 index 000000000..1964a15cb --- /dev/null +++ b/packages/livox_driver/src/comm/cache_index.h @@ -0,0 +1,55 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_CACHE_INDEX_H_ +#define LIVOX_ROS_DRIVER_CACHE_INDEX_H_ + +#include +#include +#include +#include + +#include "comm/comm.h" + +namespace livox_ros { + +class CacheIndex { + public: + CacheIndex(); + int8_t GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index); + int8_t GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index); + int8_t GenerateIndexKey( + const uint8_t livox_lidar_type, const uint32_t handle, std::string& key); + int8_t LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index); + void ResetIndex(LidarDevice* lidar); + + private: + std::mutex index_mutex_; + std::map map_index_; /* key:handle/slot, val:index */ + std::array index_cache_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_CACHE_INDEX_H_ diff --git a/packages/livox_driver/src/comm/comm.cpp b/packages/livox_driver/src/comm/comm.cpp new file mode 100644 index 000000000..08d8dca9c --- /dev/null +++ b/packages/livox_driver/src/comm/comm.cpp @@ -0,0 +1,69 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "comm/comm.h" +#include +#include + +namespace livox_ros { + +/** Common function --------------------------------------------------------- */ +bool IsFilePathValid(const char* path_str) { + int str_len = strlen(path_str); + + if ((str_len > kPathStrMinSize) && (str_len < kPathStrMaxSize)) { + return true; + } else { + return false; + } +} + +uint32_t CalculatePacketQueueSize(const double publish_freq) { + uint32_t queue_size = 10; + if (publish_freq > 10.0) { + queue_size = static_cast(publish_freq) + 1; + } + return queue_size; +} + +std::string IpNumToString(uint32_t ip_num) { + struct in_addr ip; + ip.s_addr = ip_num; + return std::string(inet_ntoa(ip)); +} + +uint32_t IpStringToNum(std::string ip_string) { + return static_cast(inet_addr(ip_string.c_str())); +} + +std::string ReplacePeriodByUnderline(std::string str) { + std::size_t pos = str.find("."); + while (pos != std::string::npos) { + str.replace(pos, 1, "_"); + pos = str.find("."); + } + return str; +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/comm/comm.h b/packages/livox_driver/src/comm/comm.h new file mode 100644 index 000000000..4a1f0ad55 --- /dev/null +++ b/packages/livox_driver/src/comm/comm.h @@ -0,0 +1,303 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER2_COMM_H_ +#define LIVOX_ROS_DRIVER2_COMM_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "lidar_imu_data_queue.h" + +namespace livox_ros { + +/** Max lidar data source num */ +const uint8_t kMaxSourceLidar = 32; + +/** Eth packet relative info parama */ +const uint32_t kMaxPointPerEthPacket = 100; +const uint32_t kMinEthPacketQueueSize = 32; /**< must be 2^n */ +const uint32_t kMaxEthPacketQueueSize = 131072; /**< must be 2^n */ +const uint32_t kImuEthPacketQueueSize = 256; + +/** Max packet length according to Ethernet MTU */ +const uint32_t KEthPacketMaxLength = 1500; +const uint32_t KEthPacketHeaderLength = 18; /**< (sizeof(LivoxEthPacket) - 1) */ +const uint32_t KCartesianPointSize = 13; +const uint32_t KSphericalPointSzie = 9; + +const uint64_t kRosTimeMax = 4294967296000000000; /**< 2^32 * 1000000000ns */ +const int64_t kPacketTimeGap = 1000000; /**< 1ms = 1000000ns */ +/**< the threshold of packet continuous */ +const int64_t kMaxPacketTimeGap = 1700000; +/**< the threshold of device disconect */ +const int64_t kDeviceDisconnectThreshold = 1000000000; +const uint32_t kNsPerSecond = 1000000000; /**< 1s = 1000000000ns */ +const uint32_t kNsTolerantFrameTimeDeviation = 1000000; /**< 1ms = 1000000ns */ +const uint32_t kRatioOfMsToNs = 1000000; /**< 1ms = 1000000ns */ + +const int kPathStrMinSize = 4; /**< Must more than 4 char */ +const int kPathStrMaxSize = 256; /**< Must less than 256 char */ +const int kBdCodeSize = 15; + +const uint32_t kPointXYZRSize = 16; +const uint32_t kPointXYZRTRSize = 18; + +const double PI = 3.14159265358979323846; + +constexpr uint32_t kMaxBufferSize = 0x8000; // 32k bytes + +/** Device Line Number **/ +const uint8_t kLineNumberDefault = 1; +const uint8_t kLineNumberMid360 = 4; +const uint8_t kLineNumberHAP = 6; + +// SDK related +typedef enum { + kIndustryLidarType = 1, + kVehicleLidarType = 2, + kDirectLidarType = 4, + kLivoxLidarType = 8 +} LidarProtoType; + +// SDK related +/** Timestamp sync mode define. */ +typedef enum { + kTimestampTypeNoSync = 0, /**< No sync signal mode. */ + kTimestampTypeGptpOrPtp = 1, /**< gPTP or PTP sync mode */ + kTimestampTypeGps = 2 /**< GPS sync mode. */ +} TimestampType; + +/** Lidar connect state */ +typedef enum { + kConnectStateOff = 0, + kConnectStateOn = 1, + kConnectStateConfig = 2, + kConnectStateSampling = 3, +} LidarConnectState; + +/** Device data source type */ +typedef enum { + kSourceRawLidar = 0, /**< Data from raw lidar. */ + kSourceRawHub = 1, /**< Data from lidar hub. */ + kSourceLvxFile, /**< Data from parse lvx file. */ + kSourceUndef, +} LidarDataSourceType; + +typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType; + +typedef enum { + kConfigDataType = 1 << 0, + kConfigScanPattern = 1 << 1, + kConfigBlindSpot = 1 << 2, + kConfigDualEmit = 1 << 3, + kConfigUnknown +} LivoxLidarConfigCodeBit; + +typedef enum { + kNoneExtrinsicParameter, + kExtrinsicParameterFromLidar, + kExtrinsicParameterFromXml +} ExtrinsicParameterType; + +typedef struct { + uint8_t lidar_type{}; +} LidarSummaryInfo; + +/** 8bytes stamp to uint64_t stamp */ +typedef union { + struct { + uint32_t low; + uint32_t high; + } stamp_word; + + uint8_t stamp_bytes[8]; + int64_t stamp; +} LdsStamp; + +#pragma pack(1) + +typedef struct { + float x; /**< X axis, Unit:m */ + float y; /**< Y axis, Unit:m */ + float z; /**< Z axis, Unit:m */ + float reflectivity; /**< Reflectivity */ + uint8_t tag; /**< Livox point tag */ + uint8_t line; /**< Laser line id */ + double timestamp; /**< Timestamp of point*/ +} LivoxPointXyzrtlt; + +typedef struct { + float x; + float y; + float z; + float intensity; + uint8_t tag; + uint8_t line; + uint64_t offset_time; +} PointXyzlt; + +typedef struct { + uint32_t handle; + uint8_t lidar_type; ////refer to LivoxLidarType + uint32_t points_num; + PointXyzlt* points; +} PointPacket; + +typedef struct { + uint64_t base_time[kMaxSourceLidar]{}; + uint8_t lidar_num{}; + PointPacket lidar_point[kMaxSourceLidar]{}; +} PointFrame; + +#pragma pack() + +typedef struct { + LidarProtoType lidar_type; + uint32_t handle; + uint64_t base_time; + uint32_t points_num; + std::vector points; +} StoragePacket; + +typedef struct { + LidarProtoType lidar_type; + uint32_t handle; + bool extrinsic_enable; + uint32_t point_num; + uint8_t data_type; + uint8_t line_num; + uint64_t time_stamp; + uint64_t point_interval; + std::vector raw_data; +} RawPacket; + +typedef struct { + StoragePacket* storage_packet; + volatile uint32_t rd_idx; + volatile uint32_t wr_idx; + uint32_t mask; + uint32_t size; /**< must be power of 2. */ +} LidarDataQueue; + +/*****************************/ +/* About Extrinsic Parameter */ +typedef struct { + float roll; /**< Roll angle, unit: degree. */ + float pitch; /**< Pitch angle, unit: degree. */ + float yaw; /**< Yaw angle, unit: degree. */ + int32_t x; /**< X translation, unit: mm. */ + int32_t y; /**< Y translation, unit: mm. */ + int32_t z; /**< Z translation, unit: mm. */ +} ExtParameter; + +typedef float TranslationVector[3]; /**< x, y, z translation, unit: mm. */ +typedef float RotationMatrix[3][3]; + +typedef struct { + TranslationVector trans; + RotationMatrix rotation; +} ExtParameterDetailed; + +typedef struct { + LidarProtoType lidar_type; + uint32_t handle; + ExtParameter param; +} LidarExtParameter; + +/** Configuration in json config file for livox lidar */ +typedef struct { + char broadcast_code[16]; + bool enable_connect; + bool enable_fan; + uint32_t return_mode; + uint32_t coordinate; + uint32_t imu_rate; + uint32_t extrinsic_parameter_source; + bool enable_high_sensitivity; +} UserRawConfig; + +typedef struct { + bool enable_fan; + uint32_t return_mode; + uint32_t coordinate; /**< 0 for CartesianCoordinate; others for SphericalCoordinate. */ + uint32_t imu_rate; + uint32_t extrinsic_parameter_source; + bool enable_high_sensitivity; + volatile uint32_t set_bits; + volatile uint32_t get_bits; +} UserConfig; + +typedef struct { + uint32_t handle; + int8_t pcl_data_type; + int8_t pattern_mode; + int32_t blind_spot_set; + int8_t dual_emit_en; + ExtParameter extrinsic_param; + volatile uint32_t set_bits; + volatile uint32_t get_bits; +} UserLivoxLidarConfig; + +/** Lidar data source info abstract */ +typedef struct { + uint8_t lidar_type; + uint32_t handle; + // union { + // uint8_t slot : 4; //slot for LivoxLidarType::kVehicleLidarType + // uint8_t handle : 4; // handle for LivoxLidarType::kIndustryLidarType + // }; + uint8_t data_src; /**< From raw lidar or livox file. */ + volatile LidarConnectState connect_state; + // DeviceInfo info; + + LidarDataQueue data; + LidarImuDataQueue imu_data; + + uint32_t firmware_ver; /**< Firmware version of lidar */ + UserLivoxLidarConfig livox_config; +} LidarDevice; + +constexpr uint32_t kMaxProductType = 10; +constexpr uint32_t kDeviceTypeLidarMid70 = 6; + +/***********************************/ +/* Global function for general use */ +bool IsFilePathValid(const char* path_str); +uint32_t CalculatePacketQueueSize(const double publish_freq); +std::string IpNumToString(uint32_t ip_num); +uint32_t IpStringToNum(std::string ip_string); +std::string ReplacePeriodByUnderline(std::string str); + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER2_COMM_H_ diff --git a/packages/livox_driver/src/comm/ldq.cpp b/packages/livox_driver/src/comm/ldq.cpp new file mode 100644 index 000000000..e7def3d97 --- /dev/null +++ b/packages/livox_driver/src/comm/ldq.cpp @@ -0,0 +1,146 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include +#include + +#include "ldq.h" + +namespace livox_ros { + +/* for pointcloud queue process */ +bool InitQueue(LidarDataQueue* queue, uint32_t queue_size) { + if (queue == nullptr) { + // ROS_WARN("RosDriver Queue: Initialization failed - invalid queue."); + return false; + } + + if (!IsPowerOf2(queue_size)) { + queue_size = RoundupPowerOf2(queue_size); + printf("Init queue, real query size:%u.\n", queue_size); + } + + if (queue->storage_packet) { + delete[] queue->storage_packet; + queue->storage_packet = nullptr; + } + + queue->storage_packet = new StoragePacket[queue_size]; + if (queue->storage_packet == nullptr) { + // ROS_WARN("RosDriver Queue: Initialization failed - failed to allocate memory."); + return false; + } + + queue->rd_idx = 0; + queue->wr_idx = 0; + queue->size = queue_size; + queue->mask = queue_size - 1; + + return true; +} + +bool DeInitQueue(LidarDataQueue* queue) { + if (queue == nullptr) { + // ROS_WARN("RosDriver Queue: Deinitialization failed - invalid queue."); + return false; + } + + if (queue->storage_packet) { + delete[] queue->storage_packet; + } + + queue->rd_idx = 0; + queue->wr_idx = 0; + queue->size = 0; + queue->mask = 0; + + return true; +} + +void ResetQueue(LidarDataQueue* queue) { + queue->rd_idx = 0; + queue->wr_idx = 0; +} + +bool QueuePrePop(LidarDataQueue* queue, StoragePacket* storage_packet) { + if (queue == nullptr || storage_packet == nullptr) { + // ROS_WARN("RosDriver Queue: Invalid pointer parameters."); + return false; + } + + if (QueueIsEmpty(queue)) { + // ROS_WARN("RosDriver Queue: Pop failed, since the queue is empty."); + return false; + } + + uint32_t rd_idx = queue->rd_idx & queue->mask; + + storage_packet->base_time = queue->storage_packet[rd_idx].base_time; + storage_packet->points_num = queue->storage_packet[rd_idx].points_num; + storage_packet->points.resize(queue->storage_packet[rd_idx].points_num); + + memcpy( + storage_packet->points.data(), + queue->storage_packet[rd_idx].points.data(), + (storage_packet->points_num) * sizeof(PointXyzlt)); + return true; +} + +void QueuePopUpdate(LidarDataQueue* queue) { queue->rd_idx++; } + +bool QueuePop(LidarDataQueue* queue, StoragePacket* storage_packet) { + if (!QueuePrePop(queue, storage_packet)) { + return false; + } + QueuePopUpdate(queue); + + return true; +} + +uint32_t QueueUsedSize(LidarDataQueue* queue) { return queue->wr_idx - queue->rd_idx; } + +uint32_t QueueUnusedSize(LidarDataQueue* queue) { return (queue->size - QueueUsedSize(queue)); } + +bool QueueIsFull(LidarDataQueue* queue) { return ((queue->wr_idx - queue->rd_idx) > queue->mask); } + +bool QueueIsEmpty(LidarDataQueue* queue) { return (queue->rd_idx == queue->wr_idx); } + +uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t* data, const uint64_t base_time) { + uint32_t wr_idx = queue->wr_idx & queue->mask; + PointPacket* lidar_point_data = reinterpret_cast(data); + queue->storage_packet[wr_idx].base_time = base_time; + queue->storage_packet[wr_idx].points_num = lidar_point_data->points_num; + + queue->storage_packet[wr_idx].points.clear(); + queue->storage_packet[wr_idx].points.resize(lidar_point_data->points_num); + memcpy( + queue->storage_packet[wr_idx].points.data(), + lidar_point_data->points, + sizeof(PointXyzlt) * (lidar_point_data->points_num)); + + queue->wr_idx++; + return 1; +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/comm/ldq.h b/packages/livox_driver/src/comm/ldq.h new file mode 100644 index 000000000..2fea8fb30 --- /dev/null +++ b/packages/livox_driver/src/comm/ldq.h @@ -0,0 +1,64 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LDQ_H_ +#define LIVOX_ROS_DRIVER_LDQ_H_ + +#include +#include + +#include "comm/comm.h" + +namespace livox_ros { + +inline static bool IsPowerOf2(uint32_t size) { return (size != 0) && ((size & (size - 1)) == 0); } + +inline static uint32_t RoundupPowerOf2(uint32_t size) { + uint32_t power2_val = 0; + for (int i = 0; i < 32; i++) { + power2_val = ((uint32_t)1) << i; + if (size <= power2_val) { + break; + } + } + + return power2_val; +} + +/** queue operate function */ +bool InitQueue(LidarDataQueue* queue, uint32_t queue_size); +bool DeInitQueue(LidarDataQueue* queue); +void ResetQueue(LidarDataQueue* queue); +bool QueuePrePop(LidarDataQueue* queue, StoragePacket* storage_packet); +void QueuePopUpdate(LidarDataQueue* queue); +bool QueuePop(LidarDataQueue* queue, StoragePacket* storage_packet); +uint32_t QueueUsedSize(LidarDataQueue* queue); +uint32_t QueueUnusedSize(LidarDataQueue* queue); +bool QueueIsFull(LidarDataQueue* queue); +bool QueueIsEmpty(LidarDataQueue* queue); +uint32_t QueuePushAny(LidarDataQueue* queue, uint8_t* data, const uint64_t base_time); + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LDQ_H_ diff --git a/packages/livox_driver/src/comm/lidar_imu_data_queue.cpp b/packages/livox_driver/src/comm/lidar_imu_data_queue.cpp new file mode 100644 index 000000000..92a5ce3b0 --- /dev/null +++ b/packages/livox_driver/src/comm/lidar_imu_data_queue.cpp @@ -0,0 +1,70 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lidar_imu_data_queue.h" + +namespace livox_ros { + +void LidarImuDataQueue::Push(ImuData* imu_data) { + ImuData data; + data.lidar_type = imu_data->lidar_type; + data.handle = imu_data->handle; + data.time_stamp = imu_data->time_stamp; + + data.gyro_x = imu_data->gyro_x; + data.gyro_y = imu_data->gyro_y; + data.gyro_z = imu_data->gyro_z; + + data.acc_x = imu_data->acc_x; + data.acc_y = imu_data->acc_y; + data.acc_z = imu_data->acc_z; + + std::lock_guard lock(mutex_); + imu_data_queue_.push_back(std::move(data)); +} + +bool LidarImuDataQueue::Pop(ImuData& imu_data) { + std::lock_guard lock(mutex_); + if (imu_data_queue_.empty()) { + return false; + } + imu_data = imu_data_queue_.front(); + imu_data_queue_.pop_front(); + return true; +} + +bool LidarImuDataQueue::Empty() { + std::lock_guard lock(mutex_); + return imu_data_queue_.empty(); +} + +void LidarImuDataQueue::Clear() { + std::list tmp_imu_data_queue; + { + std::lock_guard lock(mutex_); + imu_data_queue_.swap(tmp_imu_data_queue); + } +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/comm/lidar_imu_data_queue.h b/packages/livox_driver/src/comm/lidar_imu_data_queue.h new file mode 100644 index 000000000..196695841 --- /dev/null +++ b/packages/livox_driver/src/comm/lidar_imu_data_queue.h @@ -0,0 +1,76 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_ +#define LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_ + +#include +#include +#include + +namespace livox_ros { + +// Based on the IMU Data Type in Livox communication protocol +// TODO: add a link to the protocol +typedef struct { + float gyro_x; /**< Gyroscope X axis, Unit:rad/s */ + float gyro_y; /**< Gyroscope Y axis, Unit:rad/s */ + float gyro_z; /**< Gyroscope Z axis, Unit:rad/s */ + float acc_x; /**< Accelerometer X axis, Unit:g */ + float acc_y; /**< Accelerometer Y axis, Unit:g */ + float acc_z; /**< Accelerometer Z axis, Unit:g */ +} RawImuPoint; + +typedef struct { + uint8_t lidar_type; + uint32_t handle; + uint8_t slot; + // union { + // uint8_t handle; + // uint8_t slot; + // }; + uint64_t time_stamp; + float gyro_x; /**< Gyroscope X axis, Unit:rad/s */ + float gyro_y; /**< Gyroscope Y axis, Unit:rad/s */ + float gyro_z; /**< Gyroscope Z axis, Unit:rad/s */ + float acc_x; /**< Accelerometer X axis, Unit:g */ + float acc_y; /**< Accelerometer Y axis, Unit:g */ + float acc_z; /**< Accelerometer Z axis, Unit:g */ +} ImuData; + +class LidarImuDataQueue { + public: + void Push(ImuData* imu_data); + bool Pop(ImuData& imu_data); + bool Empty(); + void Clear(); + + private: + std::mutex mutex_; + std::list imu_data_queue_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_ diff --git a/packages/livox_driver/src/comm/pub_handler.cpp b/packages/livox_driver/src/comm/pub_handler.cpp new file mode 100644 index 000000000..9fd9d834d --- /dev/null +++ b/packages/livox_driver/src/comm/pub_handler.cpp @@ -0,0 +1,450 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "pub_handler.h" + +#include +#include +#include +#include + +namespace livox_ros { + +std::atomic PubHandler::is_timestamp_sync_; + +PubHandler& pub_handler() { + static PubHandler handler; + return handler; +} + +void PubHandler::Init() {} + +void PubHandler::Uninit() { + if (lidar_listen_id_ > 0) { + LivoxLidarRemovePointCloudObserver(lidar_listen_id_); + lidar_listen_id_ = 0; + } + + RequestExit(); + + if (point_process_thread_ && point_process_thread_->joinable()) { + point_process_thread_->join(); + point_process_thread_ = nullptr; + } else { + /* */ + } +} + +void PubHandler::RequestExit() { is_quit_.store(true); } + +void PubHandler::SetPointCloudConfig(const double publish_freq) { + publish_interval_ = (kNsPerSecond / (publish_freq * 10)) * 10; + publish_interval_tolerance_ = publish_interval_ - kNsTolerantFrameTimeDeviation; + publish_interval_ms_ = publish_interval_ / kRatioOfMsToNs; + if (!point_process_thread_) { + point_process_thread_ = std::make_shared(&PubHandler::RawDataProcess, this); + } + return; +} + +void PubHandler::SetImuDataCallback(ImuDataCallback cb, void* client_data) { + imu_client_data_ = client_data; + imu_callback_ = cb; +} + +void PubHandler::AddLidarsExtParam(LidarExtParameter& lidar_param) { + std::unique_lock lock(packet_mutex_); + uint32_t id = 0; + GetLidarId(lidar_param.lidar_type, lidar_param.handle, id); + lidar_extrinsics_[id] = lidar_param; +} + +void PubHandler::ClearAllLidarsExtrinsicParams() { + std::unique_lock lock(packet_mutex_); + lidar_extrinsics_.clear(); +} + +void PubHandler::SetPointCloudsCallback(PointCloudsCallback cb, void* client_data) { + pub_client_data_ = client_data; + points_callback_ = cb; + lidar_listen_id_ = LivoxLidarAddPointCloudObserver(OnLivoxLidarPointCloudCallback, this); +} + +void PubHandler::OnLivoxLidarPointCloudCallback( + uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data) { + PubHandler* self = (PubHandler*)client_data; + if (!self) { + return; + } + + if (data->time_type != kTimestampTypeNoSync) { + is_timestamp_sync_.store(true); + } else { + is_timestamp_sync_.store(false); + } + + if (data->data_type == kLivoxLidarImuData) { + if (self->imu_callback_) { + RawImuPoint* imu = (RawImuPoint*)data->data; + ImuData imu_data; + imu_data.lidar_type = static_cast(LidarProtoType::kLivoxLidarType); + imu_data.handle = handle; + imu_data.time_stamp = + GetEthPacketTimestamp(data->time_type, data->timestamp, sizeof(data->timestamp)); + imu_data.gyro_x = imu->gyro_x; + imu_data.gyro_y = imu->gyro_y; + imu_data.gyro_z = imu->gyro_z; + imu_data.acc_x = imu->acc_x; + imu_data.acc_y = imu->acc_y; + imu_data.acc_z = imu->acc_z; + self->imu_callback_(&imu_data, self->imu_client_data_); + } + return; + } + RawPacket packet = {}; + packet.handle = handle; + packet.lidar_type = LidarProtoType::kLivoxLidarType; + packet.extrinsic_enable = false; + if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeIndustrialHAP) { + packet.line_num = kLineNumberHAP; + } else if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeMid360) { + packet.line_num = kLineNumberMid360; + } else { + packet.line_num = kLineNumberDefault; + } + packet.data_type = data->data_type; + packet.point_num = data->dot_num; + packet.point_interval = data->time_interval * 100 / data->dot_num; // ns + packet.time_stamp = + GetEthPacketTimestamp(data->time_type, data->timestamp, sizeof(data->timestamp)); + uint32_t length = data->length - sizeof(LivoxLidarEthernetPacket) + 1; + packet.raw_data.insert(packet.raw_data.end(), data->data, data->data + length); + { + std::unique_lock lock(self->packet_mutex_); + self->raw_packet_queue_.push_back(packet); + } + self->packet_condition_.notify_one(); + + return; +} + +void PubHandler::PublishPointCloud() { + // publish point + if (points_callback_) { + points_callback_(&frame_, pub_client_data_); + } + return; +} + +void PubHandler::CheckTimer(uint32_t id) { + if (PubHandler::is_timestamp_sync_.load()) { // Enable time synchronization + auto& process_handler = lidar_process_handlers_[id]; + uint64_t recent_time_ms = process_handler->GetRecentTimeStamp() / kRatioOfMsToNs; + if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) { + return; + } + + uint64_t diff = process_handler->GetRecentTimeStamp() - process_handler->GetLidarBaseTime(); + if (diff < publish_interval_tolerance_) { + return; + } + + frame_.base_time[frame_.lidar_num] = process_handler->GetLidarBaseTime(); + points_[id].clear(); + process_handler->GetLidarPointClouds(points_[id]); + if (points_[id].empty()) { + return; + } + PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num]; + lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO: + lidar_point.handle = id; + lidar_point.points_num = points_[id].size(); + lidar_point.points = points_[id].data(); + frame_.lidar_num++; + + if (frame_.lidar_num != 0) { + PublishPointCloud(); + frame_.lidar_num = 0; + } + } else { // Disable time synchronization + auto now_time = std::chrono::high_resolution_clock::now(); + // First Set + static bool first = true; + if (first) { + last_pub_time_ = now_time; + first = false; + return; + } + if (now_time - last_pub_time_ < std::chrono::nanoseconds(publish_interval_)) { + return; + } + last_pub_time_ += std::chrono::nanoseconds(publish_interval_); + for (auto& process_handler : lidar_process_handlers_) { + frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime(); + uint32_t handle = process_handler.first; + points_[handle].clear(); + process_handler.second->GetLidarPointClouds(points_[handle]); + if (points_[handle].empty()) { + continue; + } + PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num]; + lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO: + lidar_point.handle = handle; + lidar_point.points_num = points_[handle].size(); + lidar_point.points = points_[handle].data(); + frame_.lidar_num++; + } + PublishPointCloud(); + frame_.lidar_num = 0; + } + return; +} + +void PubHandler::RawDataProcess() { + RawPacket raw_data; + while (!is_quit_.load()) { + { + std::unique_lock lock(packet_mutex_); + if (raw_packet_queue_.empty()) { + packet_condition_.wait_for(lock, std::chrono::milliseconds(500)); + if (raw_packet_queue_.empty()) { + continue; + } + } + raw_data = raw_packet_queue_.front(); + raw_packet_queue_.pop_front(); + } + uint32_t id = 0; + GetLidarId(raw_data.lidar_type, raw_data.handle, id); + if (lidar_process_handlers_.find(id) == lidar_process_handlers_.end()) { + lidar_process_handlers_[id].reset(new LidarPubHandler()); + } + auto& process_handler = lidar_process_handlers_[id]; + if (lidar_extrinsics_.find(id) != lidar_extrinsics_.end()) { + lidar_process_handlers_[id]->SetLidarsExtParam(lidar_extrinsics_[id]); + } + process_handler->PointCloudProcess(raw_data); + CheckTimer(id); + } +} + +bool PubHandler::GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id) { + if (lidar_type == kLivoxLidarType) { + id = handle; + return true; + } + return false; +} + +uint64_t PubHandler::GetEthPacketTimestamp( + uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size) { + LdsStamp time; + memcpy(time.stamp_bytes, time_stamp, size); + + if (timestamp_type == kTimestampTypeGptpOrPtp || timestamp_type == kTimestampTypeGps) { + return time.stamp; + } + + return std::chrono::high_resolution_clock::now().time_since_epoch().count(); +} + +/*******************************/ +/* LidarPubHandler Definitions*/ +LidarPubHandler::LidarPubHandler() : is_set_extrinsic_params_(false) {} + +uint64_t LidarPubHandler::GetLidarBaseTime() { + if (points_clouds_.empty()) { + return 0; + } + return points_clouds_.at(0).offset_time; +} + +void LidarPubHandler::GetLidarPointClouds(std::vector& points_clouds) { + std::lock_guard lock(mutex_); + points_clouds.swap(points_clouds_); +} + +uint64_t LidarPubHandler::GetRecentTimeStamp() { + if (points_clouds_.empty()) { + return 0; + } + return points_clouds_.back().offset_time; +} + +uint32_t LidarPubHandler::GetLidarPointCloudsSize() { + std::lock_guard lock(mutex_); + return points_clouds_.size(); +} + +// convert to standard format and extrinsic compensate +void LidarPubHandler::PointCloudProcess(RawPacket& pkt) { + if (pkt.lidar_type == LidarProtoType::kLivoxLidarType) { + LivoxLidarPointCloudProcess(pkt); + } else { + static bool flag = false; + if (!flag) { + std::cout << "error, unsupported protocol type: " << static_cast(pkt.lidar_type) + << std::endl; + flag = true; + } + } +} + +void LidarPubHandler::LivoxLidarPointCloudProcess(RawPacket& pkt) { + switch (pkt.data_type) { + case kLivoxLidarCartesianCoordinateHighData: + ProcessCartesianHighPoint(pkt); + break; + case kLivoxLidarCartesianCoordinateLowData: + ProcessCartesianLowPoint(pkt); + break; + case kLivoxLidarSphericalCoordinateData: + ProcessSphericalPoint(pkt); + break; + default: + std::cout << "unknown data type: " << static_cast(pkt.data_type) << " !!" + << std::endl; + break; + } +} + +void LidarPubHandler::SetLidarsExtParam(LidarExtParameter lidar_param) { + if (is_set_extrinsic_params_) { + return; + } + extrinsic_.trans[0] = lidar_param.param.x; + extrinsic_.trans[1] = lidar_param.param.y; + extrinsic_.trans[2] = lidar_param.param.z; + + double cos_roll = cos(static_cast(lidar_param.param.roll * PI / 180.0)); + double cos_pitch = cos(static_cast(lidar_param.param.pitch * PI / 180.0)); + double cos_yaw = cos(static_cast(lidar_param.param.yaw * PI / 180.0)); + double sin_roll = sin(static_cast(lidar_param.param.roll * PI / 180.0)); + double sin_pitch = sin(static_cast(lidar_param.param.pitch * PI / 180.0)); + double sin_yaw = sin(static_cast(lidar_param.param.yaw * PI / 180.0)); + + extrinsic_.rotation[0][0] = cos_pitch * cos_yaw; + extrinsic_.rotation[0][1] = sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw; + extrinsic_.rotation[0][2] = cos_roll * sin_pitch * cos_yaw + sin_roll * sin_yaw; + + extrinsic_.rotation[1][0] = cos_pitch * sin_yaw; + extrinsic_.rotation[1][1] = sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw; + extrinsic_.rotation[1][2] = cos_roll * sin_pitch * sin_yaw - sin_roll * cos_yaw; + + extrinsic_.rotation[2][0] = -sin_pitch; + extrinsic_.rotation[2][1] = sin_roll * cos_pitch; + extrinsic_.rotation[2][2] = cos_roll * cos_pitch; + + is_set_extrinsic_params_ = true; +} + +void LidarPubHandler::ProcessCartesianHighPoint(RawPacket& pkt) { + LivoxLidarCartesianHighRawPoint* raw = (LivoxLidarCartesianHighRawPoint*)pkt.raw_data.data(); + PointXyzlt point = {}; + for (uint32_t i = 0; i < pkt.point_num; i++) { + if (pkt.extrinsic_enable) { + point.x = raw[i].x / 1000.0; + point.y = raw[i].y / 1000.0; + point.z = raw[i].z / 1000.0; + } else { + point.x = (raw[i].x * extrinsic_.rotation[0][0] + raw[i].y * extrinsic_.rotation[0][1] + + raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) + / 1000.0; + point.y = (raw[i].x * extrinsic_.rotation[1][0] + raw[i].y * extrinsic_.rotation[1][1] + + raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) + / 1000.0; + point.z = (raw[i].x * extrinsic_.rotation[2][0] + raw[i].y * extrinsic_.rotation[2][1] + + raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) + / 1000.0; + } + point.intensity = raw[i].reflectivity; + point.line = i % pkt.line_num; + point.tag = raw[i].tag; + point.offset_time = pkt.time_stamp + i * pkt.point_interval; + std::lock_guard lock(mutex_); + points_clouds_.push_back(point); + } +} + +void LidarPubHandler::ProcessCartesianLowPoint(RawPacket& pkt) { + LivoxLidarCartesianLowRawPoint* raw = (LivoxLidarCartesianLowRawPoint*)pkt.raw_data.data(); + PointXyzlt point = {}; + for (uint32_t i = 0; i < pkt.point_num; i++) { + if (pkt.extrinsic_enable) { + point.x = raw[i].x / 100.0; + point.y = raw[i].y / 100.0; + point.z = raw[i].z / 100.0; + } else { + point.x = (raw[i].x * extrinsic_.rotation[0][0] + raw[i].y * extrinsic_.rotation[0][1] + + raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) + / 100.0; + point.y = (raw[i].x * extrinsic_.rotation[1][0] + raw[i].y * extrinsic_.rotation[1][1] + + raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) + / 100.0; + point.z = (raw[i].x * extrinsic_.rotation[2][0] + raw[i].y * extrinsic_.rotation[2][1] + + raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) + / 100.0; + } + point.intensity = raw[i].reflectivity; + point.line = i % pkt.line_num; + point.tag = raw[i].tag; + point.offset_time = pkt.time_stamp + i * pkt.point_interval; + std::lock_guard lock(mutex_); + points_clouds_.push_back(point); + } +} + +void LidarPubHandler::ProcessSphericalPoint(RawPacket& pkt) { + LivoxLidarSpherPoint* raw = (LivoxLidarSpherPoint*)pkt.raw_data.data(); + PointXyzlt point = {}; + for (uint32_t i = 0; i < pkt.point_num; i++) { + double radius = raw[i].depth / 1000.0; + double theta = raw[i].theta / 100.0 / 180 * PI; + double phi = raw[i].phi / 100.0 / 180 * PI; + double src_x = radius * sin(theta) * cos(phi); + double src_y = radius * sin(theta) * sin(phi); + double src_z = radius * cos(theta); + if (pkt.extrinsic_enable) { + point.x = src_x; + point.y = src_y; + point.z = src_z; + } else { + point.x = src_x * extrinsic_.rotation[0][0] + src_y * extrinsic_.rotation[0][1] + + src_z * extrinsic_.rotation[0][2] + (extrinsic_.trans[0] / 1000.0); + point.y = src_x * extrinsic_.rotation[1][0] + src_y * extrinsic_.rotation[1][1] + + src_z * extrinsic_.rotation[1][2] + (extrinsic_.trans[1] / 1000.0); + point.z = src_x * extrinsic_.rotation[2][0] + src_y * extrinsic_.rotation[2][1] + + src_z * extrinsic_.rotation[2][2] + (extrinsic_.trans[2] / 1000.0); + } + + point.intensity = raw[i].reflectivity; + point.line = i % pkt.line_num; + point.tag = raw[i].tag; + point.offset_time = pkt.time_stamp + i * pkt.point_interval; + std::lock_guard lock(mutex_); + points_clouds_.push_back(point); + } +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/comm/pub_handler.h b/packages/livox_driver/src/comm/pub_handler.h new file mode 100644 index 000000000..b2a06f7fa --- /dev/null +++ b/packages/livox_driver/src/comm/pub_handler.h @@ -0,0 +1,132 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_DRIVER_PUB_HANDLER_H_ +#define LIVOX_DRIVER_PUB_HANDLER_H_ + +#include +#include +#include // std::condition_variable +#include +#include +#include +#include +#include // std::mutex +#include + +#include "livox_lidar_def.h" +#include "livox_lidar_api.h" +#include "comm/comm.h" + +namespace livox_ros { + +class LidarPubHandler { + public: + LidarPubHandler(); + ~LidarPubHandler() {} + + void PointCloudProcess(RawPacket& pkt); + void SetLidarsExtParam(LidarExtParameter param); + void GetLidarPointClouds(std::vector& points_clouds); + + uint64_t GetRecentTimeStamp(); + uint32_t GetLidarPointCloudsSize(); + uint64_t GetLidarBaseTime(); + + private: + void LivoxLidarPointCloudProcess(RawPacket& pkt); + void ProcessCartesianHighPoint(RawPacket& pkt); + void ProcessCartesianLowPoint(RawPacket& pkt); + void ProcessSphericalPoint(RawPacket& pkt); + std::vector points_clouds_; + ExtParameterDetailed extrinsic_ = {{0, 0, 0}, {{1, 0, 0}, {0, 1, 1}, {0, 0, 1}}}; + std::mutex mutex_; + std::atomic_bool is_set_extrinsic_params_; +}; + +class PubHandler { + public: + using PointCloudsCallback = std::function; + using ImuDataCallback = std::function; + using TimePoint = std::chrono::high_resolution_clock::time_point; + + PubHandler() {} + + ~PubHandler() { Uninit(); } + + void Uninit(); + void RequestExit(); + void Init(); + void SetPointCloudConfig(const double publish_freq); + void SetPointCloudsCallback(PointCloudsCallback cb, void* client_data); + void AddLidarsExtParam(LidarExtParameter& extrinsic_params); + void ClearAllLidarsExtrinsicParams(); + void SetImuDataCallback(ImuDataCallback cb, void* client_data); + + private: + // thread to process raw data + void RawDataProcess(); + std::atomic is_quit_{false}; + std::shared_ptr point_process_thread_; + std::mutex packet_mutex_; + std::condition_variable packet_condition_; + + // publish callback + void CheckTimer(uint32_t id); + void PublishPointCloud(); + static void OnLivoxLidarPointCloudCallback( + uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data); + + static bool GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id); + static uint64_t GetEthPacketTimestamp( + uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size); + + PointCloudsCallback points_callback_; + void* pub_client_data_ = nullptr; + + ImuDataCallback imu_callback_; + void* imu_client_data_ = nullptr; + + PointFrame frame_; + + std::deque raw_packet_queue_; + + // pub config + uint64_t publish_interval_ = 100000000; // 100 ms + uint64_t publish_interval_tolerance_ = 100000000; // 100 ms + uint64_t publish_interval_ms_ = 100; // 100 ms + TimePoint last_pub_time_; + + std::map> lidar_process_handlers_; + std::map> points_; + std::map lidar_extrinsics_; + static std::atomic is_timestamp_sync_; + uint16_t lidar_listen_id_ = 0; +}; + +PubHandler& pub_handler(); + +} // namespace livox_ros + +#endif // LIVOX_DRIVER_PUB_HANDLER_H_ diff --git a/packages/livox_driver/src/comm/semaphore.cpp b/packages/livox_driver/src/comm/semaphore.cpp new file mode 100644 index 000000000..dec45409a --- /dev/null +++ b/packages/livox_driver/src/comm/semaphore.cpp @@ -0,0 +1,41 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "semaphore.h" + +namespace livox_ros { + +void Semaphore::Signal() { + std::unique_lock lock(mutex_); + ++count_; + cv_.notify_one(); +} + +void Semaphore::Wait() { + std::unique_lock lock(mutex_); + cv_.wait(lock, [=] { return count_ > 0; }); + --count_; +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/comm/semaphore.h b/packages/livox_driver/src/comm/semaphore.h new file mode 100644 index 000000000..da5323824 --- /dev/null +++ b/packages/livox_driver/src/comm/semaphore.h @@ -0,0 +1,48 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_SEMAPHORE_H_ +#define LIVOX_ROS_DRIVER_SEMAPHORE_H_ + +#include +#include + +namespace livox_ros { + +class Semaphore { + public: + explicit Semaphore(int count = 0) : count_(count) {} + void Signal(); + void Wait(); + int GetCount() { return count_; } + + private: + std::mutex mutex_; + std::condition_variable cv_; + volatile int count_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_SEMAPHORE_H_ diff --git a/packages/livox_driver/src/driver_node.cpp b/packages/livox_driver/src/driver_node.cpp new file mode 100644 index 000000000..54ab816e1 --- /dev/null +++ b/packages/livox_driver/src/driver_node.cpp @@ -0,0 +1,39 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "driver_node.h" +#include "lddc.h" + +namespace livox_ros { + +DriverNode& DriverNode::GetNode() noexcept { return *this; } + +DriverNode::~DriverNode() { + lddc_ptr_->lds_->RequestExit(); + exit_signal_.set_value(); + pointclouddata_poll_thread_->join(); + imudata_poll_thread_->join(); +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/driver_node.h b/packages/livox_driver/src/driver_node.h new file mode 100644 index 000000000..b92dedf0d --- /dev/null +++ b/packages/livox_driver/src/driver_node.h @@ -0,0 +1,78 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_DRIVER_NODE_H +#define LIVOX_DRIVER_NODE_H + +#include "include/ros_headers.h" + +namespace livox_ros { + +class Lddc; + +#ifdef BUILDING_ROS1 +class DriverNode final : public ros::NodeHandle { + public: + DriverNode() = default; + DriverNode(const DriverNode&) = delete; + ~DriverNode(); + DriverNode& operator=(const DriverNode&) = delete; + + DriverNode& GetNode() noexcept; + + void PointCloudDataPollThread(); + void ImuDataPollThread(); + + std::unique_ptr lddc_ptr_; + std::shared_ptr pointclouddata_poll_thread_; + std::shared_ptr imudata_poll_thread_; + std::shared_future future_; + std::promise exit_signal_; +}; + +#elif defined BUILDING_ROS2 +class DriverNode final : public rclcpp::Node { + public: + explicit DriverNode(const rclcpp::NodeOptions& options); + DriverNode(const DriverNode&) = delete; + ~DriverNode(); + DriverNode& operator=(const DriverNode&) = delete; + + DriverNode& GetNode() noexcept; + + private: + void PointCloudDataPollThread(); + void ImuDataPollThread(); + + std::unique_ptr lddc_ptr_; + std::shared_ptr pointclouddata_poll_thread_; + std::shared_ptr imudata_poll_thread_; + std::shared_future future_; + std::promise exit_signal_; +}; +#endif + +} // namespace livox_ros + +#endif // LIVOX_DRIVER_NODE_H diff --git a/packages/livox_driver/src/include/livox_driver.h b/packages/livox_driver/src/include/livox_driver.h new file mode 100644 index 000000000..b40bba3bb --- /dev/null +++ b/packages/livox_driver/src/include/livox_driver.h @@ -0,0 +1,39 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER2_INClUDE_H_ +#define LIVOX_ROS_DRIVER2_INClUDE_H_ + +#define LIVOX_ROS_DRIVER2_VER_MAJOR 1 +#define LIVOX_ROS_DRIVER2_VER_MINOR 2 +#define LIVOX_ROS_DRIVER2_VER_PATCH 4 + +#define GET_STRING(n) GET_STRING_DIRECT(n) +#define GET_STRING_DIRECT(n) #n + +#define LIVOX_ROS_DRIVER2_VERSION_STRING \ + GET_STRING(LIVOX_ROS_DRIVER2_VER_MAJOR) \ + "." GET_STRING(LIVOX_ROS_DRIVER2_VER_MINOR) "." GET_STRING(LIVOX_ROS_DRIVER2_VER_PATCH) + +#endif // LIVOX_ROS_DRIVER2_INClUDE_H_ diff --git a/packages/livox_driver/src/include/ros1_headers.h b/packages/livox_driver/src/include/ros1_headers.h new file mode 100644 index 000000000..a1012c801 --- /dev/null +++ b/packages/livox_driver/src/include/ros1_headers.h @@ -0,0 +1,53 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +// Denoting headers specifically used for building ROS1 Driver. + +#ifndef ROS1_HEADERS_H_ +#define ROS1_HEADERS_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include "livox_driver/CustomMsg.h" +#include "livox_driver/CustomPoint.h" + +#define DRIVER_DEBUG(node, ...) ROS_DEBUG(__VA_ARGS__) +#define DRIVER_INFO(node, ...) ROS_INFO(__VA_ARGS__) +#define DRIVER_WARN(node, ...) ROS_WARN(__VA_ARGS__) +#define DRIVER_ERROR(node, ...) ROS_ERROR(__VA_ARGS__) +#define DRIVER_FATAL(node, ...) ROS_FATAL(__VA_ARGS__) + +#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) ROS_DEBUG_##EXTRA(__VA_ARGS__) +#define DRIVER_INFO_EXTRA(node, EXTRA, ...) ROS_INFO_##EXTRA(__VA_ARGS__) +#define DRIVER_WARN_EXTRA(node, EXTRA, ...) ROS_WARN_##EXTRA(__VA_ARGS__) +#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) ROS_ERROR_##EXTRA(__VA_ARGS__) +#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) ROS_FATAL_##EXTRA(__VA_ARGS__) + +#endif // ROS1_HEADERS_H_ diff --git a/packages/livox_driver/src/include/ros2_headers.h b/packages/livox_driver/src/include/ros2_headers.h new file mode 100644 index 000000000..069b40323 --- /dev/null +++ b/packages/livox_driver/src/include/ros2_headers.h @@ -0,0 +1,52 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +// Denoting headers specifically used for building ROS1 Driver. + +#ifndef ROS2_HEADERS_H_ +#define ROS2_HEADERS_H_ + +#include +#include + +#include +#include +#include +#include +#include "livox_driver/msg/custom_point.hpp" +#include "livox_driver/msg/custom_msg.hpp" + +#define DRIVER_DEBUG(node, ...) RCLCPP_DEBUG((node).get_logger(), __VA_ARGS__) +#define DRIVER_INFO(node, ...) RCLCPP_INFO((node).get_logger(), __VA_ARGS__) +#define DRIVER_WARN(node, ...) RCLCPP_WARN((node).get_logger(), __VA_ARGS__) +#define DRIVER_ERROR(node, ...) RCLCPP_ERROR((node).get_logger(), __VA_ARGS__) +#define DRIVER_FATAL(node, ...) RCLCPP_FATAL((node).get_logger(), __VA_ARGS__) + +#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) RCLCPP_DEBUG_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_INFO_EXTRA(node, EXTRA, ...) RCLCPP_INFO_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_WARN_EXTRA(node, EXTRA, ...) RCLCPP_WARN_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) RCLCPP_ERROR_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) RCLCPP_FATAL_##EXTRA((node).get_logger(), __VA_ARGS__) + +#endif // ROS2_HEADERS_H_ diff --git a/packages/livox_driver/src/include/ros_headers.h b/packages/livox_driver/src/include/ros_headers.h new file mode 100644 index 000000000..05993089f --- /dev/null +++ b/packages/livox_driver/src/include/ros_headers.h @@ -0,0 +1,34 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef ROS_HEADERS_H_ +#define ROS_HEADERS_H_ + +#ifdef BUILDING_ROS1 +#include "ros1_headers.h" +#elif defined BUILDING_ROS2 +#include "ros2_headers.h" +#endif + +#endif // ROS_HEADERS_H_ diff --git a/packages/livox_driver/src/lddc.cpp b/packages/livox_driver/src/lddc.cpp new file mode 100644 index 000000000..87890cefa --- /dev/null +++ b/packages/livox_driver/src/lddc.cpp @@ -0,0 +1,718 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lddc.h" +#include "comm/ldq.h" +#include "comm/comm.h" + +#include +#include +#include +#include +#include + +#include "include/ros_headers.h" + +#include "driver_node.h" +#include "lds_lidar.h" + +namespace livox_ros { + +/** Lidar Data Distribute Control--------------------------------------------*/ +#ifdef BUILDING_ROS1 +Lddc::Lddc( + int format, int multi_topic, int data_src, int output_type, double frq, std::string& frame_id, + bool lidar_bag, bool imu_bag) : + transfer_format_(format), + use_multi_topic_(multi_topic), + data_src_(data_src), + output_type_(output_type), + publish_frq_(frq), + frame_id_(frame_id), + enable_lidar_bag_(lidar_bag), + enable_imu_bag_(imu_bag) { + publish_period_ns_ = kNsPerSecond / publish_frq_; + lds_ = nullptr; + memset(private_pub_, 0, sizeof(private_pub_)); + memset(private_imu_pub_, 0, sizeof(private_imu_pub_)); + global_pub_ = nullptr; + global_imu_pub_ = nullptr; + cur_node_ = nullptr; + bag_ = nullptr; +} +#elif defined BUILDING_ROS2 +Lddc::Lddc( + int format, int multi_topic, int data_src, int output_type, double frq, std::string& frame_id) : + transfer_format_(format), + use_multi_topic_(multi_topic), + data_src_(data_src), + output_type_(output_type), + publish_frq_(frq), + frame_id_(frame_id) { + publish_period_ns_ = kNsPerSecond / publish_frq_; + lds_ = nullptr; +#if 0 + bag_ = nullptr; +#endif +} +#endif + +Lddc::~Lddc() { +#ifdef BUILDING_ROS1 + if (global_pub_) { + delete global_pub_; + } + + if (global_imu_pub_) { + delete global_imu_pub_; + } +#endif + + PrepareExit(); + +#ifdef BUILDING_ROS1 + for (uint32_t i = 0; i < kMaxSourceLidar; i++) { + if (private_pub_[i]) { + delete private_pub_[i]; + } + } + + for (uint32_t i = 0; i < kMaxSourceLidar; i++) { + if (private_imu_pub_[i]) { + delete private_imu_pub_[i]; + } + } +#endif + std::cout << "lddc destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl; +} + +int Lddc::RegisterLds(Lds* lds) { + if (lds_ == nullptr) { + lds_ = lds; + return 0; + } else { + return -1; + } +} + +void Lddc::DistributePointCloudData(void) { + if (!lds_) { + std::cout << "lds is not registered" << std::endl; + return; + } + if (lds_->IsRequestExit()) { + std::cout << "DistributePointCloudData is RequestExit" << std::endl; + 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)) { + continue; + } + PollingLidarPointCloudData(lidar_id, lidar); + } +} + +void Lddc::DistributeImuData(void) { + if (!lds_) { + std::cout << "lds is not registered" << std::endl; + return; + } + if (lds_->IsRequestExit()) { + std::cout << "DistributeImuData is RequestExit" << std::endl; + return; + } + + lds_->imu_semaphore_.Wait(); + 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)) { + continue; + } + PollingLidarImuData(lidar_id, lidar); + } +} + +void Lddc::PollingLidarPointCloudData(uint8_t index, LidarDevice* lidar) { + LidarDataQueue* p_queue = &lidar->data; + if (p_queue == nullptr || p_queue->storage_packet == nullptr) { + return; + } + + while (!lds_->IsRequestExit() && !QueueIsEmpty(p_queue)) { + if (kPointCloud2Msg == transfer_format_) { + PublishPointcloud2(p_queue, index); + } else if (kLivoxCustomMsg == transfer_format_) { + PublishCustomPointcloud(p_queue, index); + } else if (kPclPxyziMsg == transfer_format_) { + PublishPclMsg(p_queue, index); + } + } +} + +void Lddc::PollingLidarImuData(uint8_t index, LidarDevice* lidar) { + LidarImuDataQueue& p_queue = lidar->imu_data; + while (!lds_->IsRequestExit() && !p_queue.Empty()) { + PublishImuData(p_queue, index); + } +} + +void Lddc::PrepareExit(void) { +#ifdef BUILDING_ROS1 + if (bag_) { + DRIVER_INFO(*cur_node_, "Waiting to save the bag file!"); + bag_->close(); + DRIVER_INFO(*cur_node_, "Save the bag file successfully!"); + bag_ = nullptr; + } +#endif + if (lds_) { + lds_->PrepareExit(); + lds_ = nullptr; + } +} + +void Lddc::PublishPointcloud2(LidarDataQueue* queue, uint8_t index) { + while (!QueueIsEmpty(queue)) { + StoragePacket pkg; + QueuePop(queue, &pkg); + if (pkg.points.empty()) { + printf("Publish point cloud2 failed, the pkg points is empty.\n"); + continue; + } + + PointCloud2 cloud; + uint64_t timestamp = 0; + InitPointcloud2Msg(pkg, cloud, timestamp); + PublishPointcloud2Data(index, timestamp, cloud); + } +} + +void Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint8_t index) { + while (!QueueIsEmpty(queue)) { + StoragePacket pkg; + QueuePop(queue, &pkg); + if (pkg.points.empty()) { + printf("Publish custom point cloud failed, the pkg points is empty.\n"); + continue; + } + + CustomMsg livox_msg; + InitCustomMsg(livox_msg, pkg, index); + FillPointsToCustomMsg(livox_msg, pkg); + PublishCustomPointData(livox_msg, index); + } +} + +/* for pcl::pxyzi */ +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; + 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"); + continue; + } + + PointCloud cloud; + uint64_t timestamp = 0; + InitPclMsg(pkg, cloud, timestamp); + FillPointsToPclMsg(pkg, cloud); + PublishPclData(index, timestamp, cloud); + } + return; +} + +void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud) { + cloud.header.frame_id.assign(frame_id_); + cloud.height = 1; + cloud.width = 0; + cloud.fields.resize(7); + cloud.fields[0].offset = 0; + cloud.fields[0].name = "x"; + cloud.fields[0].count = 1; + cloud.fields[0].datatype = PointField::FLOAT32; + cloud.fields[1].offset = 4; + cloud.fields[1].name = "y"; + cloud.fields[1].count = 1; + cloud.fields[1].datatype = PointField::FLOAT32; + cloud.fields[2].offset = 8; + cloud.fields[2].name = "z"; + cloud.fields[2].count = 1; + cloud.fields[2].datatype = PointField::FLOAT32; + cloud.fields[3].offset = 12; + cloud.fields[3].name = "intensity"; + cloud.fields[3].count = 1; + cloud.fields[3].datatype = PointField::FLOAT32; + cloud.fields[4].offset = 16; + cloud.fields[4].name = "tag"; + cloud.fields[4].count = 1; + cloud.fields[4].datatype = PointField::UINT8; + cloud.fields[5].offset = 17; + cloud.fields[5].name = "line"; + cloud.fields[5].count = 1; + cloud.fields[5].datatype = PointField::UINT8; + cloud.fields[6].offset = 18; + cloud.fields[6].name = "timestamp"; + cloud.fields[6].count = 1; + cloud.fields[6].datatype = PointField::FLOAT64; + cloud.point_step = sizeof(LivoxPointXyzrtlt); +} + +void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp) { + InitPointcloud2MsgHeader(cloud); + + cloud.point_step = sizeof(LivoxPointXyzrtlt); + + cloud.width = pkg.points_num; + cloud.row_step = cloud.width * cloud.point_step; + + cloud.is_bigendian = false; + cloud.is_dense = true; + + if (!pkg.points.empty()) { + timestamp = pkg.base_time; + } + +#ifdef BUILDING_ROS1 + cloud.header.stamp = ros::Time(timestamp / 1000000000.0); +#elif defined BUILDING_ROS2 + cloud.header.stamp = rclcpp::Time(timestamp); +#endif + + std::vector points; + for (size_t i = 0; i < pkg.points_num; ++i) { + LivoxPointXyzrtlt point; + point.x = pkg.points[i].x; + point.y = pkg.points[i].y; + point.z = pkg.points[i].z; + point.reflectivity = pkg.points[i].intensity; + point.tag = pkg.points[i].tag; + point.line = pkg.points[i].line; + point.timestamp = static_cast(pkg.points[i].offset_time); + points.push_back(std::move(point)); + } + cloud.data.resize(pkg.points_num * sizeof(LivoxPointXyzrtlt)); + memcpy(cloud.data.data(), points.data(), pkg.points_num * sizeof(LivoxPointXyzrtlt)); +} + +void Lddc::PublishPointcloud2Data( + const uint8_t index, const uint64_t timestamp, const PointCloud2& cloud) { +#ifdef BUILDING_ROS1 + PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); +#elif defined BUILDING_ROS2 + Publisher::SharedPtr publisher_ptr = + std::dynamic_pointer_cast >(GetCurrentPublisher(index)); +#endif + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(cloud); + } else { +#ifdef BUILDING_ROS1 + if (bag_ && enable_lidar_bag_) { + bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud); + } +#endif + } +} + +void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index) { + livox_msg.header.frame_id.assign(frame_id_); + +#ifdef BUILDING_ROS1 + static uint32_t msg_seq = 0; + livox_msg.header.seq = msg_seq; + ++msg_seq; +#endif + + uint64_t timestamp = 0; + if (!pkg.points.empty()) { + timestamp = pkg.base_time; + } + livox_msg.timebase = timestamp; + +#ifdef BUILDING_ROS1 + livox_msg.header.stamp = ros::Time(timestamp / 1000000000.0); +#elif defined BUILDING_ROS2 + livox_msg.header.stamp = rclcpp::Time(timestamp); +#endif + + livox_msg.point_num = pkg.points_num; + 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_msg.lidar_id = 0; + } +} + +void Lddc::FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg) { + uint32_t points_num = pkg.points_num; + const std::vector& points = pkg.points; + for (uint32_t i = 0; i < points_num; ++i) { + CustomPoint point; + point.x = points[i].x; + point.y = points[i].y; + point.z = points[i].z; + point.reflectivity = points[i].intensity; + point.tag = points[i].tag; + point.line = points[i].line; + point.offset_time = static_cast(points[i].offset_time - pkg.base_time); + + livox_msg.points.push_back(std::move(point)); + } +} + +void Lddc::PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index) { +#ifdef BUILDING_ROS1 + PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); +#elif defined BUILDING_ROS2 + Publisher::SharedPtr publisher_ptr = + std::dynamic_pointer_cast >(GetCurrentPublisher(index)); +#endif + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(livox_msg); + } else { +#ifdef BUILDING_ROS1 + if (bag_ && enable_lidar_bag_) { + bag_->write( + publisher_ptr->getTopic(), ros::Time(livox_msg.timebase / 1000000000.0), livox_msg); + } +#endif + } +} + +void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp) { +#ifdef BUILDING_ROS1 + cloud.header.frame_id.assign(frame_id_); + cloud.height = 1; + cloud.width = pkg.points_num; + + if (!pkg.points.empty()) { + timestamp = pkg.base_time; + } + 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; +#endif + return; +} + +void Lddc::FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg) { +#ifdef BUILDING_ROS1 + if (pkg.points.empty()) { + return; + } + + uint32_t points_num = pkg.points_num; + const std::vector& points = pkg.points; + for (uint32_t i = 0; i < points_num; ++i) { + pcl::PointXYZI point; + point.x = points[i].x; + point.y = points[i].y; + point.z = points[i].z; + point.intensity = points[i].intensity; + + 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; +#endif + return; +} + +void Lddc::PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud) { +#ifdef BUILDING_ROS1 + PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); + if (kOutputToRos == output_type_) { + publisher_ptr->publish(cloud); + } else { + if (bag_ && enable_lidar_bag_) { + bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud); + } + } +#elif defined BUILDING_ROS2 + std::cout << "warning: pcl::PointCloud is not supported in ROS2, " + << "please check code logic" << std::endl; +#endif + return; +} + +void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp) { + imu_msg.header.frame_id = "livox_frame"; + + timestamp = imu_data.time_stamp; +#ifdef BUILDING_ROS1 + imu_msg.header.stamp = ros::Time(timestamp / 1000000000.0); // to ros time stamp +#elif defined BUILDING_ROS2 + imu_msg.header.stamp = rclcpp::Time(timestamp); // to ros time stamp +#endif + + imu_msg.angular_velocity.x = imu_data.gyro_x; + imu_msg.angular_velocity.y = imu_data.gyro_y; + imu_msg.angular_velocity.z = imu_data.gyro_z; + imu_msg.linear_acceleration.x = imu_data.acc_x; + imu_msg.linear_acceleration.y = imu_data.acc_y; + imu_msg.linear_acceleration.z = imu_data.acc_z; +} + +void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index) { + ImuData imu_data; + if (!imu_data_queue.Pop(imu_data)) { + // printf("Publish imu data failed, imu data queue pop failed.\n"); + return; + } + + ImuMsg imu_msg; + uint64_t timestamp; + InitImuMsg(imu_data, imu_msg, timestamp); + +#ifdef BUILDING_ROS1 + PublisherPtr publisher_ptr = GetCurrentImuPublisher(index); +#elif defined BUILDING_ROS2 + Publisher::SharedPtr publisher_ptr = + std::dynamic_pointer_cast >(GetCurrentImuPublisher(index)); +#endif + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(imu_msg); + } else { +#ifdef BUILDING_ROS1 + if (bag_ && enable_imu_bag_) { + bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), imu_msg); + } +#endif + } +} + +#ifdef BUILDING_ROS2 +std::shared_ptr Lddc::CreatePublisher( + uint8_t msg_type, std::string& topic_name, uint32_t queue_size) { + if (kPointCloud2Msg == msg_type) { + DRIVER_INFO(*cur_node_, "%s publish use PointCloud2 format", topic_name.c_str()); + return cur_node_->create_publisher(topic_name, queue_size); + } else if (kLivoxCustomMsg == msg_type) { + DRIVER_INFO(*cur_node_, "%s publish use livox custom format", topic_name.c_str()); + return cur_node_->create_publisher(topic_name, queue_size); + } +#if 0 + else if (kPclPxyziMsg == msg_type) { + DRIVER_INFO(*cur_node_, + "%s publish use pcl PointXYZI format", topic_name.c_str()); + return cur_node_->create_publisher(topic_name, queue_size); + } +#endif + else if (kLivoxImuMsg == msg_type) { + DRIVER_INFO(*cur_node_, "%s publish use imu format", topic_name.c_str()); + return cur_node_->create_publisher(topic_name, queue_size); + } else { + PublisherPtr null_publisher(nullptr); + return null_publisher; + } +} +#endif + +#ifdef BUILDING_ROS1 +PublisherPtr Lddc::GetCurrentPublisher(uint8_t index) { + ros::Publisher** pub = nullptr; + uint32_t queue_size = kMinEthPacketQueueSize; + + if (use_multi_topic_) { + pub = &private_pub_[index]; + queue_size = queue_size / 8; // queue size is 4 for only one lidar + } else { + pub = &global_pub_; + queue_size = queue_size * 8; // shared queue size is 256, for all lidars + } + + if (*pub == nullptr) { + char name_str[48]; + memset(name_str, 0, sizeof(name_str)); + if (use_multi_topic_) { + std::string ip_string = IpNumToString(lds_->lidars_[index].handle); + snprintf( + name_str, + sizeof(name_str), + "livox/lidar_%s", + ReplacePeriodByUnderline(ip_string).c_str()); + DRIVER_INFO(*cur_node_, "Support multi topics."); + } else { + DRIVER_INFO(*cur_node_, "Support only one topic."); + snprintf(name_str, sizeof(name_str), "livox/lidar"); + } + + *pub = new ros::Publisher; + if (kPointCloud2Msg == transfer_format_) { + **pub = cur_node_->GetNode().advertise(name_str, queue_size); + DRIVER_INFO( + *cur_node_, + "%s publish use PointCloud2 format, set ROS publisher queue size %d", + name_str, + queue_size); + } else if (kLivoxCustomMsg == transfer_format_) { + **pub = cur_node_->GetNode().advertise(name_str, queue_size); + DRIVER_INFO( + *cur_node_, + "%s publish use livox custom format, set ROS publisher queue size %d", + name_str, + queue_size); + } else if (kPclPxyziMsg == transfer_format_) { + **pub = cur_node_->GetNode().advertise(name_str, queue_size); + DRIVER_INFO( + *cur_node_, + "%s publish use pcl PointXYZI format, set ROS publisher queue " + "size %d", + name_str, + queue_size); + } + } + + return *pub; +} + +PublisherPtr Lddc::GetCurrentImuPublisher(uint8_t handle) { + ros::Publisher** pub = nullptr; + uint32_t queue_size = kMinEthPacketQueueSize; + + if (use_multi_topic_) { + pub = &private_imu_pub_[handle]; + queue_size = queue_size * 2; // queue size is 64 for only one lidar + } else { + pub = &global_imu_pub_; + queue_size = queue_size * 8; // shared queue size is 256, for all lidars + } + + if (*pub == nullptr) { + char name_str[48]; + memset(name_str, 0, sizeof(name_str)); + if (use_multi_topic_) { + DRIVER_INFO(*cur_node_, "Support multi topics."); + std::string ip_string = IpNumToString(lds_->lidars_[handle].handle); + snprintf( + name_str, + sizeof(name_str), + "livox/imu_%s", + ReplacePeriodByUnderline(ip_string).c_str()); + } else { + DRIVER_INFO(*cur_node_, "Support only one topic."); + snprintf(name_str, sizeof(name_str), "livox/imu"); + } + + *pub = new ros::Publisher; + **pub = cur_node_->GetNode().advertise(name_str, queue_size); + DRIVER_INFO( + *cur_node_, + "%s publish imu data, set ROS publisher queue size %d", + name_str, + queue_size); + } + + return *pub; +} +#elif defined BUILDING_ROS2 +std::shared_ptr Lddc::GetCurrentPublisher(uint8_t handle) { + uint32_t queue_size = kMinEthPacketQueueSize; + if (use_multi_topic_) { + if (!private_pub_[handle]) { + char name_str[48]; + memset(name_str, 0, sizeof(name_str)); + + std::string ip_string = IpNumToString(lds_->lidars_[handle].handle); + snprintf( + name_str, + sizeof(name_str), + "livox/lidar_%s", + ReplacePeriodByUnderline(ip_string).c_str()); + std::string topic_name(name_str); + queue_size = queue_size * 2; // queue size is 64 for only one lidar + private_pub_[handle] = CreatePublisher(transfer_format_, topic_name, queue_size); + } + return private_pub_[handle]; + } else { + if (!global_pub_) { + std::string topic_name("livox/lidar"); + queue_size = queue_size * 8; // shared queue size is 256, for all lidars + global_pub_ = CreatePublisher(transfer_format_, topic_name, queue_size); + } + return global_pub_; + } +} + +std::shared_ptr Lddc::GetCurrentImuPublisher(uint8_t handle) { + uint32_t queue_size = kMinEthPacketQueueSize; + if (use_multi_topic_) { + if (!private_imu_pub_[handle]) { + char name_str[48]; + memset(name_str, 0, sizeof(name_str)); + std::string ip_string = IpNumToString(lds_->lidars_[handle].handle); + snprintf( + name_str, + sizeof(name_str), + "livox/imu_%s", + ReplacePeriodByUnderline(ip_string).c_str()); + std::string topic_name(name_str); + queue_size = queue_size * 2; // queue size is 64 for only one lidar + private_imu_pub_[handle] = CreatePublisher(kLivoxImuMsg, topic_name, queue_size); + } + return private_imu_pub_[handle]; + } else { + if (!global_imu_pub_) { + std::string topic_name("livox/imu"); + queue_size = queue_size * 8; // shared queue size is 256, for all lidars + global_imu_pub_ = CreatePublisher(kLivoxImuMsg, topic_name, queue_size); + } + return global_imu_pub_; + } +} +#endif + +void Lddc::CreateBagFile(const std::string& file_name) { +#ifdef BUILDING_ROS1 + if (!bag_) { + bag_ = new rosbag::Bag; + bag_->open(file_name, rosbag::bagmode::Write); + DRIVER_INFO(*cur_node_, "Create bag file :%s!", file_name.c_str()); + } +#endif +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/lddc.h b/packages/livox_driver/src/lddc.h new file mode 100644 index 000000000..9ce5f91ef --- /dev/null +++ b/packages/livox_driver/src/lddc.h @@ -0,0 +1,167 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER2_LDDC_H_ +#define LIVOX_ROS_DRIVER2_LDDC_H_ + +#include "include/livox_driver.h" + +#include "driver_node.h" +#include "lds.h" + +namespace livox_ros { + +/** Send pointcloud message Data to ros subscriber or save them in rosbag file */ +typedef enum { + kOutputToRos = 0, + kOutputToRosBagFile = 1, +} DestinationOfMessageOutput; + +/** The message type of transfer */ +typedef enum { + kPointCloud2Msg = 0, + kLivoxCustomMsg = 1, + kPclPxyziMsg = 2, + kLivoxImuMsg = 3, +} TransferType; + +/** Type-Definitions based on ROS versions */ +#ifdef BUILDING_ROS1 +using Publisher = ros::Publisher; +using PublisherPtr = ros::Publisher*; +using PointCloud2 = sensor_msgs::PointCloud2; +using PointField = sensor_msgs::PointField; +using CustomMsg = livox_driver::CustomMsg; +using CustomPoint = livox_driver::CustomPoint; +using ImuMsg = sensor_msgs::Imu; +#elif defined BUILDING_ROS2 +template +using Publisher = rclcpp::Publisher; +using PublisherPtr = std::shared_ptr; +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointField = sensor_msgs::msg::PointField; +using CustomMsg = livox_driver::msg::CustomMsg; +using CustomPoint = livox_driver::msg::CustomPoint; +using ImuMsg = sensor_msgs::msg::Imu; +#endif + +using PointCloud = pcl::PointCloud; + +class DriverNode; + +class Lddc final { + public: +#ifdef BUILDING_ROS1 + Lddc( + int format, int multi_topic, int data_src, int output_type, double frq, + std::string& frame_id, bool lidar_bag, bool imu_bag); +#elif defined BUILDING_ROS2 + Lddc( + int format, int multi_topic, int data_src, int output_type, double frq, + std::string& frame_id); +#endif + ~Lddc(); + + int RegisterLds(Lds* lds); + void DistributePointCloudData(void); + void DistributeImuData(void); + void CreateBagFile(const std::string& file_name); + void PrepareExit(void); + + uint8_t GetTransferFormat(void) { return transfer_format_; } + uint8_t IsMultiTopic(void) { return use_multi_topic_; } + void SetRosNode(livox_ros::DriverNode* node) { cur_node_ = node; } + + // void SetRosPub(ros::Publisher *pub) { global_pub_ = pub; }; // NOT USED + void SetPublishFrq(uint32_t frq) { publish_frq_ = frq; } + + public: + Lds* lds_; + + private: + void PollingLidarPointCloudData(uint8_t index, LidarDevice* lidar); + void PollingLidarImuData(uint8_t index, LidarDevice* lidar); + + void PublishPointcloud2(LidarDataQueue* queue, uint8_t index); + void PublishCustomPointcloud(LidarDataQueue* queue, uint8_t index); + void PublishPclMsg(LidarDataQueue* queue, uint8_t index); + + void PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index); + + void InitPointcloud2MsgHeader(PointCloud2& cloud); + void InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint64_t& timestamp); + void PublishPointcloud2Data(const uint8_t index, uint64_t timestamp, const PointCloud2& cloud); + + void InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index); + void FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg); + void PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index); + + void InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp); + void FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg); + void PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud); + + void InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp); + + void FillPointsToPclMsg(PointCloud& pcl_msg, LivoxPointXyzrtlt* src_point, uint32_t num); + void FillPointsToCustomMsg( + CustomMsg& livox_msg, LivoxPointXyzrtlt* src_point, uint32_t num, uint32_t offset_time, + uint32_t point_interval, uint32_t echo_num); + +#ifdef BUILDING_ROS2 + PublisherPtr CreatePublisher(uint8_t msg_type, std::string& topic_name, uint32_t queue_size); +#endif + + PublisherPtr GetCurrentPublisher(uint8_t index); + PublisherPtr GetCurrentImuPublisher(uint8_t index); + + private: + uint8_t transfer_format_; + uint8_t use_multi_topic_; + uint8_t data_src_; + uint8_t output_type_; + double publish_frq_; + uint32_t publish_period_ns_; + std::string frame_id_; + +#ifdef BUILDING_ROS1 + bool enable_lidar_bag_; + bool enable_imu_bag_; + PublisherPtr private_pub_[kMaxSourceLidar]; + PublisherPtr global_pub_; + PublisherPtr private_imu_pub_[kMaxSourceLidar]; + PublisherPtr global_imu_pub_; + rosbag::Bag* bag_; +#elif defined BUILDING_ROS2 + PublisherPtr private_pub_[kMaxSourceLidar]; + PublisherPtr global_pub_; + PublisherPtr private_imu_pub_[kMaxSourceLidar]; + PublisherPtr global_imu_pub_; +#endif + + livox_ros::DriverNode* cur_node_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER2_LDDC_H_ diff --git a/packages/livox_driver/src/lds.cpp b/packages/livox_driver/src/lds.cpp new file mode 100644 index 000000000..2648eb512 --- /dev/null +++ b/packages/livox_driver/src/lds.cpp @@ -0,0 +1,206 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include +#include +#include +#include +#include +#include + +#include "lds.h" +#include "comm/ldq.h" + +namespace livox_ros { + +CacheIndex Lds::cache_index_; + +/* Member function --------------------------------------------------------- */ +Lds::Lds(const double publish_freq, const uint8_t data_src) : + lidar_count_(kMaxSourceLidar), + pcd_semaphore_(0), + imu_semaphore_(0), + publish_freq_(publish_freq), + data_src_(data_src), + request_exit_(false) { + ResetLds(data_src_); +} + +Lds::~Lds() { + lidar_count_ = 0; + ResetLds(0); + printf("lds destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"); +} + +void Lds::ResetLidar(LidarDevice* lidar, uint8_t data_src) { + // cache_index_.ResetIndex(lidar); + DeInitQueue(&lidar->data); + lidar->imu_data.Clear(); + + lidar->data_src = data_src; + lidar->connect_state = kConnectStateOff; +} + +void Lds::SetLidarDataSrc(LidarDevice* lidar, uint8_t data_src) { lidar->data_src = data_src; } + +void Lds::ResetLds(uint8_t data_src) { + lidar_count_ = kMaxSourceLidar; + for (uint32_t i = 0; i < kMaxSourceLidar; i++) { + ResetLidar(&lidars_[i], data_src); + } +} + +void Lds::RequestExit() { request_exit_ = true; } + +bool Lds::IsAllQueueEmpty() { + for (int i = 0; i < lidar_count_; i++) { + if (!QueueIsEmpty(&lidars_[i].data)) { + return false; + } + } + return true; +} + +bool Lds::IsAllQueueReadStop() { + for (int i = 0; i < lidar_count_; i++) { + uint32_t data_size = QueueUsedSize(&lidars_[i].data); + if (data_size) { + return false; + } + } + return true; +} + +void Lds::StorageImuData(ImuData* imu_data) { + uint32_t device_num = 0; + 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); + 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; + } + + LidarDevice* p_lidar = &lidars_[index]; + LidarImuDataQueue* imu_queue = &p_lidar->imu_data; + imu_queue->Push(imu_data); + if (!imu_queue->Empty()) { + if (imu_semaphore_.GetCount() <= 0) { + imu_semaphore_.Signal(); + } + } +} + +void Lds::StorageLvxPointData(PointFrame* frame) { + if (frame == nullptr) { + return; + } + + uint8_t lidar_number = frame->lidar_num; + for (uint i = 0; i < lidar_number; ++i) { + PointPacket& lidar_point = frame->lidar_point[i]; + + uint64_t base_time = frame->base_time[i]; + 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); + continue; + } + + lidars_[index].connect_state = kConnectStateSampling; + + PushLidarData(&lidar_point, index, base_time); + } +} + +void Lds::StoragePointData(PointFrame* frame) { + if (frame == nullptr) { + return; + } + + uint8_t lidar_number = frame->lidar_num; + for (uint i = 0; i < lidar_number; ++i) { + PointPacket& lidar_point = frame->lidar_point[i]; + // printf("StoragePointData, lidar_type:%u, point_num:%lu.\n", lidar_point.lidar_type, + // lidar_point.points_num); + + uint64_t base_time = frame->base_time[i]; + + 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; + } + PushLidarData(&lidar_point, index, base_time); + } +} + +void Lds::PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time) { + if (lidar_data == nullptr) { + return; + } + + LidarDevice* p_lidar = &lidars_[index]; + LidarDataQueue* queue = &p_lidar->data; + + 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); + } + + if (!QueueIsFull(queue)) { + QueuePushAny(queue, (uint8_t*)lidar_data, base_time); + if (!QueueIsEmpty(queue)) { + if (pcd_semaphore_.GetCount() <= 0) { + pcd_semaphore_.Signal(); + } + } + } else { + if (pcd_semaphore_.GetCount() <= 0) { + pcd_semaphore_.Signal(); + } + } +} + +void Lds::PrepareExit(void) {} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/lds.h b/packages/livox_driver/src/lds.h new file mode 100644 index 000000000..937400014 --- /dev/null +++ b/packages/livox_driver/src/lds.h @@ -0,0 +1,85 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +// livox lidar data source + +#ifndef LIVOX_ROS_DRIVER_LDS_H_ +#define LIVOX_ROS_DRIVER_LDS_H_ + +#include + +#include "comm/semaphore.h" +#include "comm/comm.h" +#include "comm/cache_index.h" + +namespace livox_ros { +/** + * Lidar data source abstract. + */ +class Lds { + public: + Lds(const double publish_freq, const uint8_t data_src); + virtual ~Lds(); + + void StorageImuData(ImuData* imu_data); + void StoragePointData(PointFrame* frame); + void StorageLvxPointData(PointFrame* frame); + + int8_t GetHandle(const uint8_t lidar_type, const PointPacket* lidar_point); + void PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint64_t base_time); + + static void ResetLidar(LidarDevice* lidar, uint8_t data_src); + static void SetLidarDataSrc(LidarDevice* lidar, uint8_t data_src); + void ResetLds(uint8_t data_src); + + void RequestExit(); + + bool IsAllQueueEmpty(); + bool IsAllQueueReadStop(); + + void CleanRequestExit() { request_exit_ = false; } + bool IsRequestExit() { return request_exit_; } + virtual void PrepareExit(void); + + // get publishing frequency + double GetLdsFrequency() { return publish_freq_; } + + public: + uint8_t lidar_count_; /**< Lidar access handle. */ + LidarDevice lidars_[kMaxSourceLidar]; /**< The index is the handle */ + Semaphore pcd_semaphore_; + Semaphore imu_semaphore_; + static CacheIndex cache_index_; + + protected: + double publish_freq_; + uint8_t data_src_; + + private: + volatile bool request_exit_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LDS_H_ diff --git a/packages/livox_driver/src/lds_lidar.cpp b/packages/livox_driver/src/lds_lidar.cpp new file mode 100644 index 000000000..d6ff8ee69 --- /dev/null +++ b/packages/livox_driver/src/lds_lidar.cpp @@ -0,0 +1,211 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lds_lidar.h" + +#include +#include +#include +#include +#include + +#ifdef WIN32 +#include +#include +#pragma comment(lib, "Ws2_32.lib") +#else +#include +#include +#include +#include +#endif // WIN32 + +#include "comm/comm.h" +#include "comm/pub_handler.h" + +#include "parse_cfg_file/parse_cfg_file.h" +#include "parse_cfg_file/parse_livox_lidar_cfg.h" + +#include "call_back/lidar_common_callback.h" +#include "call_back/livox_lidar_callback.h" + +using namespace std; + +namespace livox_ros { + +/** Const varible ------------------------------------------------------------*/ +/** For callback use only */ +LdsLidar* g_lds_ldiar = nullptr; + +/** Global function for common use -------------------------------------------*/ + +/** Lds lidar function -------------------------------------------------------*/ +LdsLidar::LdsLidar(double publish_freq) : + Lds(publish_freq, kSourceRawLidar), + auto_connect_mode_(true), + whitelist_count_(0), + is_initialized_(false) { + memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_)); + ResetLdsLidar(); +} + +LdsLidar::~LdsLidar() {} + +void LdsLidar::ResetLdsLidar(void) { ResetLds(kSourceRawLidar); } + +bool LdsLidar::InitLdsLidar(const std::string& path_name) { + if (is_initialized_) { + printf("Lds is already inited!\n"); + return false; + } + + if (g_lds_ldiar == nullptr) { + g_lds_ldiar = this; + } + + path_ = path_name; + if (!InitLidars()) { + return false; + } + SetLidarPubHandle(); + if (!Start()) { + return false; + } + is_initialized_ = true; + return true; +} + +bool LdsLidar::InitLidars() { + if (!ParseSummaryConfig()) { + return false; + } + std::cout << "config lidar type: " << static_cast(lidar_summary_info_.lidar_type) + << std::endl; + + if (lidar_summary_info_.lidar_type & kLivoxLidarType) { + if (!InitLivoxLidar()) { + return false; + } + } + return true; +} + +bool LdsLidar::Start() { + if (lidar_summary_info_.lidar_type & kLivoxLidarType) { + if (!LivoxLidarStart()) { + return false; + } + } + return true; +} + +bool LdsLidar::ParseSummaryConfig() { + return ParseCfgFile(path_).ParseSummaryInfo(lidar_summary_info_); +} + +bool LdsLidar::InitLivoxLidar() { +#ifdef BUILDING_ROS2 + DisableLivoxSdkConsoleLogger(); +#endif + + // parse user config + LivoxLidarConfigParser parser(path_); + std::vector user_configs; + if (!parser.Parse(user_configs)) { + std::cout << "failed to parse user-defined config" << std::endl; + } + + // SDK initialization + if (!LivoxLidarSdkInit(path_.c_str())) { + std::cout << "Failed to init livox lidar sdk." << std::endl; + return false; + } + + // 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; + continue; + } + LidarDevice* p_lidar = &(g_lds_ldiar->lidars_[index]); + p_lidar->lidar_type = kLivoxLidarType; + p_lidar->livox_config = config; + p_lidar->handle = config.handle; + + LidarExtParameter lidar_param; + lidar_param.handle = config.handle; + lidar_param.lidar_type = kLivoxLidarType; + if (config.pcl_data_type == kLivoxLidarCartesianCoordinateLowData) { + // temporary resolution + lidar_param.param.roll = config.extrinsic_param.roll; + lidar_param.param.pitch = config.extrinsic_param.pitch; + lidar_param.param.yaw = config.extrinsic_param.yaw; + lidar_param.param.x = config.extrinsic_param.x / 10; + lidar_param.param.y = config.extrinsic_param.y / 10; + lidar_param.param.z = config.extrinsic_param.z / 10; + } else { + lidar_param.param.roll = config.extrinsic_param.roll; + lidar_param.param.pitch = config.extrinsic_param.pitch; + lidar_param.param.yaw = config.extrinsic_param.yaw; + lidar_param.param.x = config.extrinsic_param.x; + lidar_param.param.y = config.extrinsic_param.y; + lidar_param.param.z = config.extrinsic_param.z; + } + pub_handler().AddLidarsExtParam(lidar_param); + } + + SetLivoxLidarInfoChangeCallback(LivoxLidarCallback::LidarInfoChangeCallback, g_lds_ldiar); + return true; +} + +void LdsLidar::SetLidarPubHandle() { + pub_handler().SetPointCloudsCallback(LidarCommonCallback::OnLidarPointClounCb, g_lds_ldiar); + pub_handler().SetImuDataCallback(LidarCommonCallback::LidarImuDataCallback, g_lds_ldiar); + + double publish_freq = Lds::GetLdsFrequency(); + pub_handler().SetPointCloudConfig(publish_freq); +} + +bool LdsLidar::LivoxLidarStart() { return true; } + +int LdsLidar::DeInitLdsLidar(void) { + if (!is_initialized_) { + printf("LiDAR data source is not exit"); + return -1; + } + + if (lidar_summary_info_.lidar_type & kLivoxLidarType) { + LivoxLidarSdkUninit(); + printf("Livox Lidar SDK Deinit completely!\n"); + } + + return 0; +} + +void LdsLidar::PrepareExit(void) { DeInitLdsLidar(); } + +} // namespace livox_ros diff --git a/packages/livox_driver/src/lds_lidar.h b/packages/livox_driver/src/lds_lidar.h new file mode 100644 index 000000000..d08ce1952 --- /dev/null +++ b/packages/livox_driver/src/lds_lidar.h @@ -0,0 +1,96 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +/** Livox LiDAR data source, data from dependent lidar */ + +#ifndef LIVOX_ROS_DRIVER_LDS_LIDAR_H_ +#define LIVOX_ROS_DRIVER_LDS_LIDAR_H_ + +#include +#include +#include + +#include "lds.h" +#include "comm/comm.h" + +#include "livox_lidar_api.h" +#include "livox_lidar_def.h" + +#include "rapidjson/document.h" + +namespace livox_ros { + +class LdsLidar final : public Lds { + public: + static LdsLidar* GetInstance(double publish_freq) { + printf("LdsLidar *GetInstance\n"); + static LdsLidar lds_lidar(publish_freq); + return &lds_lidar; + } + + bool InitLdsLidar(const std::string& path_name); + bool Start(); + + int DeInitLdsLidar(void); + + private: + LdsLidar(double publish_freq); + LdsLidar(const LdsLidar&) = delete; + ~LdsLidar(); + LdsLidar& operator=(const LdsLidar&) = delete; + + bool ParseSummaryConfig(); + + bool InitLidars(); + bool InitLivoxLidar(); // for new SDK + + bool LivoxLidarStart(); + + void ResetLdsLidar(void); + + void SetLidarPubHandle(); + + // auto connect mode + void EnableAutoConnectMode(void) { auto_connect_mode_ = true; } + void DisableAutoConnectMode(void) { auto_connect_mode_ = false; } + bool IsAutoConnectMode(void) { return auto_connect_mode_; } + + virtual void PrepareExit(void); + + public: + std::mutex config_mutex_; + + private: + std::string path_; + LidarSummaryInfo lidar_summary_info_; + + bool auto_connect_mode_; + uint32_t whitelist_count_; + volatile bool is_initialized_; + char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize]; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LDS_LIDAR_H_ diff --git a/packages/livox_driver/src/livox_driver.cpp b/packages/livox_driver/src/livox_driver.cpp new file mode 100644 index 000000000..9f3028190 --- /dev/null +++ b/packages/livox_driver/src/livox_driver.cpp @@ -0,0 +1,222 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include +#include +#include +#include +#include + +#include "include/livox_driver.h" +#include "include/ros_headers.h" +#include "driver_node.h" +#include "lddc.h" +#include "lds_lidar.h" + +using namespace livox_ros; + +#ifdef BUILDING_ROS1 +int main(int argc, char** argv) { + /** Ros related */ + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { + ros::console::notifyLoggerLevelsChanged(); + } + + ros::init(argc, argv, "livox_lidar_publisher"); + + // ros::NodeHandle livox_node; + livox_ros::DriverNode livox_node; + + DRIVER_INFO(livox_node, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING); + + /** Init default system parameter */ + int xfer_format = kPointCloud2Msg; + int multi_topic = 0; + int data_src = kSourceRawLidar; + double publish_freq = 10.0; /* Hz */ + int output_type = kOutputToRos; + std::string frame_id = "livox_frame"; + bool lidar_bag = true; + bool imu_bag = false; + + livox_node.GetNode().getParam("xfer_format", xfer_format); + livox_node.GetNode().getParam("multi_topic", multi_topic); + livox_node.GetNode().getParam("data_src", data_src); + livox_node.GetNode().getParam("publish_freq", publish_freq); + livox_node.GetNode().getParam("output_data_type", output_type); + livox_node.GetNode().getParam("frame_id", frame_id); + livox_node.GetNode().getParam("enable_lidar_bag", lidar_bag); + livox_node.GetNode().getParam("enable_imu_bag", imu_bag); + + printf("data source:%u.\n", data_src); + + if (publish_freq > 100.0) { + publish_freq = 100.0; + } else if (publish_freq < 0.5) { + publish_freq = 0.5; + } else { + publish_freq = publish_freq; + } + + livox_node.future_ = livox_node.exit_signal_.get_future(); + + /** Lidar data distribute control and lidar data source set */ + livox_node.lddc_ptr_ = std::make_unique( + xfer_format, + multi_topic, + data_src, + output_type, + publish_freq, + frame_id, + lidar_bag, + imu_bag); + livox_node.lddc_ptr_->SetRosNode(&livox_node); + + if (data_src == kSourceRawLidar) { + DRIVER_INFO(livox_node, "Data Source is raw lidar."); + + std::string user_config_path; + livox_node.getParam("user_config_path", user_config_path); + DRIVER_INFO(livox_node, "Config file : %s", user_config_path.c_str()); + + LdsLidar* read_lidar = LdsLidar::GetInstance(publish_freq); + livox_node.lddc_ptr_->RegisterLds(static_cast(read_lidar)); + + if ((read_lidar->InitLdsLidar(user_config_path))) { + DRIVER_INFO(livox_node, "Init lds lidar successfully!"); + } else { + DRIVER_ERROR(livox_node, "Init lds lidar failed!"); + } + } else { + DRIVER_ERROR(livox_node, "Invalid data src (%d), please check the launch file", data_src); + } + + livox_node.pointclouddata_poll_thread_ = + std::make_shared(&DriverNode::PointCloudDataPollThread, &livox_node); + livox_node.imudata_poll_thread_ = + std::make_shared(&DriverNode::ImuDataPollThread, &livox_node); + while (ros::ok()) { + usleep(10000); + } + + return 0; +} + +#elif defined BUILDING_ROS2 +namespace livox_ros { +DriverNode::DriverNode(const rclcpp::NodeOptions& node_options) : + Node("livox_driver_node", node_options) { + DRIVER_INFO(*this, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING); + + /** Init default system parameter */ + int xfer_format = kPointCloud2Msg; + int multi_topic = 0; + int data_src = kSourceRawLidar; + double publish_freq = 10.0; /* Hz */ + int output_type = kOutputToRos; + std::string frame_id; + + this->declare_parameter("xfer_format", xfer_format); + this->declare_parameter("multi_topic", 0); + this->declare_parameter("data_src", data_src); + this->declare_parameter("publish_freq", 10.0); + this->declare_parameter("output_data_type", output_type); + this->declare_parameter("frame_id", "frame_default"); + 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->get_parameter("xfer_format", xfer_format); + this->get_parameter("multi_topic", multi_topic); + this->get_parameter("data_src", data_src); + this->get_parameter("publish_freq", publish_freq); + this->get_parameter("output_data_type", output_type); + this->get_parameter("frame_id", frame_id); + + if (publish_freq > 100.0) { + publish_freq = 100.0; + } else if (publish_freq < 0.5) { + publish_freq = 0.5; + } else { + publish_freq = publish_freq; + } + + future_ = exit_signal_.get_future(); + + /** Lidar data distribute control and lidar data source set */ + lddc_ptr_ = std::make_unique( + xfer_format, multi_topic, data_src, output_type, publish_freq, frame_id); + lddc_ptr_->SetRosNode(this); + + if (data_src == kSourceRawLidar) { + DRIVER_INFO(*this, "Data Source is raw lidar."); + + std::string user_config_path; + this->get_parameter("user_config_path", user_config_path); + DRIVER_INFO(*this, "Config file : %s", user_config_path.c_str()); + + std::string cmdline_bd_code; + this->get_parameter("cmdline_input_bd_code", cmdline_bd_code); + + LdsLidar* read_lidar = LdsLidar::GetInstance(publish_freq); + lddc_ptr_->RegisterLds(static_cast(read_lidar)); + + if ((read_lidar->InitLdsLidar(user_config_path))) { + DRIVER_INFO(*this, "Init lds lidar success!"); + } else { + DRIVER_ERROR(*this, "Init lds lidar fail!"); + } + } else { + DRIVER_ERROR(*this, "Invalid data src (%d), please check the launch file", data_src); + } + + pointclouddata_poll_thread_ = + std::make_shared(&DriverNode::PointCloudDataPollThread, this); + imudata_poll_thread_ = std::make_shared(&DriverNode::ImuDataPollThread, this); +} + +} // namespace livox_ros + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(livox_ros::DriverNode) + +#endif // defined BUILDING_ROS2 + +void DriverNode::PointCloudDataPollThread() { + std::future_status status; + std::this_thread::sleep_for(std::chrono::seconds(3)); + do { + lddc_ptr_->DistributePointCloudData(); + status = future_.wait_for(std::chrono::microseconds(0)); + } while (status == std::future_status::timeout); +} + +void DriverNode::ImuDataPollThread() { + std::future_status status; + std::this_thread::sleep_for(std::chrono::seconds(3)); + do { + lddc_ptr_->DistributeImuData(); + status = future_.wait_for(std::chrono::microseconds(0)); + } while (status == std::future_status::timeout); +} diff --git a/packages/livox_driver/src/parse_cfg_file/parse_cfg_file.cpp b/packages/livox_driver/src/parse_cfg_file/parse_cfg_file.cpp new file mode 100644 index 000000000..5578904e4 --- /dev/null +++ b/packages/livox_driver/src/parse_cfg_file/parse_cfg_file.cpp @@ -0,0 +1,66 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "parse_cfg_file.h" + +#include +#include +#include + +namespace livox_ros { + +ParseCfgFile::ParseCfgFile(const std::string& path) : path_(path) {} + +bool ParseCfgFile::ParseSummaryInfo(LidarSummaryInfo& lidar_summary_info) { + FILE* raw_file = std::fopen(path_.c_str(), "rb"); + if (!raw_file) { + std::cout << "parse summary info failed, can not open file: " << path_ << std::endl; + return false; + } + + char read_buffer[kMaxBufferSize]; + rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer)); + rapidjson::Document doc; + do { + if (doc.ParseStream(config_file).HasParseError()) { + break; + } + if (!doc.HasMember("lidar_summary_info") || !doc["lidar_summary_info"].IsObject()) { + break; + } + const rapidjson::Value& object = doc["lidar_summary_info"]; + if (!object.HasMember("lidar_type") || !object["lidar_type"].IsUint()) { + break; + } + lidar_summary_info.lidar_type = static_cast(object["lidar_type"].GetUint()); + std::fclose(raw_file); + return true; + } while (false); + + std::cout << "parse lidar type failed." << std::endl; + std::fclose(raw_file); + return false; +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/parse_cfg_file/parse_cfg_file.h b/packages/livox_driver/src/parse_cfg_file/parse_cfg_file.h new file mode 100644 index 000000000..597573cd9 --- /dev/null +++ b/packages/livox_driver/src/parse_cfg_file/parse_cfg_file.h @@ -0,0 +1,52 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_ +#define LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_ + +#include "../comm/comm.h" + +#include "rapidjson/document.h" +#include "rapidjson/filereadstream.h" +#include "rapidjson/stringbuffer.h" + +#include +#include + +namespace livox_ros { + +class ParseCfgFile { + public: + explicit ParseCfgFile(const std::string& path); + ~ParseCfgFile() {} + + bool ParseSummaryInfo(LidarSummaryInfo& lidar_summary_info); + + private: + const std::string path_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_PARSE_CFG_FILE_H_ diff --git a/packages/livox_driver/src/parse_cfg_file/parse_livox_lidar_cfg.cpp b/packages/livox_driver/src/parse_cfg_file/parse_livox_lidar_cfg.cpp new file mode 100644 index 000000000..9ab720237 --- /dev/null +++ b/packages/livox_driver/src/parse_cfg_file/parse_livox_lidar_cfg.cpp @@ -0,0 +1,153 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "parse_livox_lidar_cfg.h" +#include + +namespace livox_ros { + +bool LivoxLidarConfigParser::Parse(std::vector& lidar_configs) { + FILE* raw_file = std::fopen(path_.c_str(), "rb"); + if (!raw_file) { + std::cout << "failed to open config file: " << path_ << std::endl; + return false; + } + + lidar_configs.clear(); + char read_buffer[kMaxBufferSize]; + rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer)); + rapidjson::Document doc; + + do { + if (doc.ParseStream(config_file).HasParseError()) { + std::cout << "failed to parse config jason" << std::endl; + break; + } + if (!doc.HasMember("lidar_configs") || !doc["lidar_configs"].IsArray() + || 0 == doc["lidar_configs"].Size()) { + std::cout << "there is no user-defined config" << std::endl; + break; + } + if (!ParseUserConfigs(doc, lidar_configs)) { + std::cout << "failed to parse basic configs" << std::endl; + break; + } + return true; + } while (false); + + std::fclose(raw_file); + return false; +} + +bool LivoxLidarConfigParser::ParseUserConfigs( + const rapidjson::Document& doc, std::vector& user_configs) { + const rapidjson::Value& lidar_configs = doc["lidar_configs"]; + for (auto& config : lidar_configs.GetArray()) { + if (!config.HasMember("ip")) { + continue; + } + UserLivoxLidarConfig user_config; + + // parse user configs + user_config.handle = IpStringToNum(std::string(config["ip"].GetString())); + if (!config.HasMember("pcl_data_type")) { + user_config.pcl_data_type = -1; + } else { + user_config.pcl_data_type = static_cast(config["pcl_data_type"].GetInt()); + } + if (!config.HasMember("pattern_mode")) { + user_config.pattern_mode = -1; + } else { + user_config.pattern_mode = static_cast(config["pattern_mode"].GetInt()); + } + if (!config.HasMember("blind_spot_set")) { + user_config.blind_spot_set = -1; + } else { + user_config.blind_spot_set = static_cast(config["blind_spot_set"].GetInt()); + } + if (!config.HasMember("dual_emit_en")) { + user_config.dual_emit_en = -1; + } else { + user_config.dual_emit_en = static_cast(config["dual_emit_en"].GetInt()); + } + if (!config.HasMember("extrinsic_parameter")) { + memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param)); + } else { + auto& value = config["extrinsic_parameter"]; + if (!ParseExtrinsics(value, user_config.extrinsic_param)) { + memset(&user_config.extrinsic_param, 0, sizeof(user_config.extrinsic_param)); + std::cout << "failed to parse extrinsic parameters, ip: " + << IpNumToString(user_config.handle) << std::endl; + } + } + user_config.set_bits = 0; + user_config.get_bits = 0; + + user_configs.push_back(user_config); + } + + if (0 == user_configs.size()) { + std::cout << "no valid base configs" << std::endl; + return false; + } + std::cout << "successfully parse base config, counts: " << user_configs.size() << std::endl; + return true; +} + +bool LivoxLidarConfigParser::ParseExtrinsics(const rapidjson::Value& value, ExtParameter& param) { + if (!value.HasMember("roll")) { + param.roll = 0.0f; + } else { + param.roll = static_cast(value["roll"].GetFloat()); + } + if (!value.HasMember("pitch")) { + param.pitch = 0.0f; + } else { + param.pitch = static_cast(value["pitch"].GetFloat()); + } + if (!value.HasMember("yaw")) { + param.yaw = 0.0f; + } else { + param.yaw = static_cast(value["yaw"].GetFloat()); + } + if (!value.HasMember("x")) { + param.x = 0; + } else { + param.x = static_cast(value["x"].GetInt()); + } + if (!value.HasMember("y")) { + param.y = 0; + } else { + param.y = static_cast(value["y"].GetInt()); + } + if (!value.HasMember("z")) { + param.z = 0; + } else { + param.z = static_cast(value["z"].GetInt()); + } + + return true; +} + +} // namespace livox_ros diff --git a/packages/livox_driver/src/parse_cfg_file/parse_livox_lidar_cfg.h b/packages/livox_driver/src/parse_cfg_file/parse_livox_lidar_cfg.h new file mode 100644 index 000000000..8ed30c485 --- /dev/null +++ b/packages/livox_driver/src/parse_cfg_file/parse_livox_lidar_cfg.h @@ -0,0 +1,58 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2022 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_ +#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_ + +#include "comm/comm.h" +#include "livox_lidar_def.h" + +#include "rapidjson/document.h" +#include "rapidjson/filereadstream.h" +#include "rapidjson/stringbuffer.h" + +#include +#include +#include + +namespace livox_ros { + +class LivoxLidarConfigParser { + public: + explicit LivoxLidarConfigParser(const std::string& path) : path_(path) {} + ~LivoxLidarConfigParser() {} + + bool Parse(std::vector& lidar_configs); + + private: + bool ParseUserConfigs( + const rapidjson::Document& doc, std::vector& user_configs); + bool ParseExtrinsics(const rapidjson::Value& value, ExtParameter& param); + + const std::string path_; +}; + +} // namespace livox_ros + +#endif // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CFG_PARSER_H_ diff --git a/scripts/rosenv.sh b/scripts/rosenv.sh index a136708f8..ecc4fd6a7 100644 --- a/scripts/rosenv.sh +++ b/scripts/rosenv.sh @@ -1,3 +1,3 @@ echo "Loading ROS environment variables..." source /opt/ros/$ROS_DISTRO/setup.sh -source packages/install/setup.sh +source install/setup.sh From 3a7fdc548bdfa92d86b4d35f857212220c8b273b Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sat, 2 Nov 2024 22:42:37 +0000 Subject: [PATCH 02/10] Update lidar config, add macvlan docker config --- macvlan.yaml | 20 +++++++++++++++++++ .../livox_driver/config/MID360_config.json | 8 ++------ 2 files changed, 22 insertions(+), 6 deletions(-) create mode 100644 macvlan.yaml diff --git a/macvlan.yaml b/macvlan.yaml new file mode 100644 index 000000000..5af5014ea --- /dev/null +++ b/macvlan.yaml @@ -0,0 +1,20 @@ +# Use together with main docker-compose.yaml file: +# docker compose -f docker-compose.yaml -f macvlan.yaml up -d + +services: + truck: + networks: + default: + macvlan: + +networks: + macvlan: + name: "${CONTAINER_NAME:-truck-${USER}}-macvlan" + driver: macvlan + driver_opts: + parent: eth0 + ipam: + config: + - subnet: 192.168.1.0/24 + gateway: 192.168.1.1 + ip_range: 192.168.1.55/32 # TODO: Use user ID or dynamic address diff --git a/packages/livox_driver/config/MID360_config.json b/packages/livox_driver/config/MID360_config.json index ad691f4c7..e30eee534 100644 --- a/packages/livox_driver/config/MID360_config.json +++ b/packages/livox_driver/config/MID360_config.json @@ -11,21 +11,17 @@ "log_data_port": 56500 }, "host_net_info" : { - "cmd_data_ip" : "192.168.1.5", + "host_ip": "192.168.1.55", "cmd_data_port": 56101, - "push_msg_ip": "192.168.1.5", "push_msg_port": 56201, - "point_data_ip": "192.168.1.5", "point_data_port": 56301, - "imu_data_ip" : "192.168.1.5", "imu_data_port": 56401, - "log_data_ip" : "", "log_data_port": 56501 } }, "lidar_configs" : [ { - "ip" : "192.168.1.12", + "ip" : "192.168.1.176", "pcl_data_type" : 1, "pattern_mode" : 0, "extrinsic_parameter" : { From bd1b96a7248a59d0b223488473a7b1d4b237d068 Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sun, 24 Nov 2024 04:55:24 +0000 Subject: [PATCH 03/10] Update driver node --- packages/livox_driver/CMakeLists.txt | 1 - .../livox_driver/config/MID360_config.json | 37 --- packages/livox_driver/launch/livox_driver.py | 132 +++++++++ .../livox_driver/launch/msg_MID360_launch.py | 55 ---- .../src/call_back/livox_lidar_callback.cpp | 3 + packages/livox_driver/src/driver_node.h | 22 -- .../livox_driver/src/include/ros_headers.h | 4 - packages/livox_driver/src/lddc.cpp | 277 +----------------- packages/livox_driver/src/lddc.h | 29 -- packages/livox_driver/src/lds_lidar.cpp | 9 +- packages/livox_driver/src/livox_driver.cpp | 97 ------ packages/truck/launch/lidar3d.yaml | 8 + packages/truck/launch/truck.yaml | 2 +- 13 files changed, 156 insertions(+), 520 deletions(-) delete mode 100644 packages/livox_driver/config/MID360_config.json create mode 100644 packages/livox_driver/launch/livox_driver.py delete mode 100644 packages/livox_driver/launch/msg_MID360_launch.py create mode 100644 packages/truck/launch/lidar3d.yaml diff --git a/packages/livox_driver/CMakeLists.txt b/packages/livox_driver/CMakeLists.txt index b07297238..7fb95e236 100644 --- a/packages/livox_driver/CMakeLists.txt +++ b/packages/livox_driver/CMakeLists.txt @@ -31,7 +31,6 @@ message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}") # ----------------------------------------------------------------------------- # Add ROS Version MACRO # ----------------------------------------------------------------------------- -add_definitions(-DBUILDING_ROS2) # find dependencies uncomment the following section in order to fill in further # dependencies manually. find_package( REQUIRED) diff --git a/packages/livox_driver/config/MID360_config.json b/packages/livox_driver/config/MID360_config.json deleted file mode 100644 index e30eee534..000000000 --- a/packages/livox_driver/config/MID360_config.json +++ /dev/null @@ -1,37 +0,0 @@ -{ - "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": "192.168.1.55", - "cmd_data_port": 56101, - "push_msg_port": 56201, - "point_data_port": 56301, - "imu_data_port": 56401, - "log_data_port": 56501 - } - }, - "lidar_configs" : [ - { - "ip" : "192.168.1.176", - "pcl_data_type" : 1, - "pattern_mode" : 0, - "extrinsic_parameter" : { - "roll": 0.0, - "pitch": 0.0, - "yaw": 0.0, - "x": 0, - "y": 0, - "z": 0 - } - } - ] -} diff --git a/packages/livox_driver/launch/livox_driver.py b/packages/livox_driver/launch/livox_driver.py new file mode 100644 index 000000000..4f52be4c0 --- /dev/null +++ b/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 diff --git a/packages/livox_driver/launch/msg_MID360_launch.py b/packages/livox_driver/launch/msg_MID360_launch.py deleted file mode 100644 index b3d3e89cc..000000000 --- a/packages/livox_driver/launch/msg_MID360_launch.py +++ /dev/null @@ -1,55 +0,0 @@ -import os - -from launch import LaunchDescription -from launch_ros.actions import Node - -################### user configure parameters for ros2 start ################### -xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format -multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic -data_src = 0 # 0-lidar, others-Invalid data src -publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc. -output_type = 0 -frame_id = "livox_frame" -lvx_file_path = "/home/livox/livox_test.lvx" -cmdline_bd_code = "livox0000000001" - -cur_path = os.path.split(os.path.realpath(__file__))[0] + "/" -cur_config_path = cur_path + "../config" -user_config_path = os.path.join(cur_config_path, "MID360_config.json") -################### user configure parameters for ros2 end ##################### - -livox_ros2_params = [ - {"xfer_format": xfer_format}, - {"multi_topic": multi_topic}, - {"data_src": data_src}, - {"publish_freq": publish_freq}, - {"output_data_type": output_type}, - {"frame_id": frame_id}, - {"lvx_file_path": lvx_file_path}, - {"user_config_path": user_config_path}, - {"cmdline_input_bd_code": cmdline_bd_code}, -] - - -def generate_launch_description(): - livox_driver = Node( - package="livox_driver", - executable="livox_driver_node", - name="livox_lidar_publisher", - output="screen", - parameters=livox_ros2_params, - ) - - return LaunchDescription( - [ - livox_driver, - # launch.actions.RegisterEventHandler( - # event_handler=launch.event_handlers.OnProcessExit( - # target_action=livox_rviz, - # on_exit=[ - # launch.actions.EmitEvent(event=launch.events.Shutdown()), - # ] - # ) - # ) - ] - ) diff --git a/packages/livox_driver/src/call_back/livox_lidar_callback.cpp b/packages/livox_driver/src/call_back/livox_lidar_callback.cpp index 6b0839ff3..e2189b851 100644 --- a/packages/livox_driver/src/call_back/livox_lidar_callback.cpp +++ b/packages/livox_driver/src/call_back/livox_lidar_callback.cpp @@ -42,6 +42,9 @@ void LivoxLidarCallback::LidarInfoChangeCallback( if (lidar_device == nullptr) { std::cout << "found lidar not defined in the user-defined config, ip: " << IpNumToString(handle) << std::endl; + // Don't register unknown lidars and skip calling SetLivoxLidarWorkMode + return; + // add lidar device uint8_t index = 0; int8_t ret = lds_lidar->cache_index_.GetFreeIndex(kLivoxLidarType, handle, index); diff --git a/packages/livox_driver/src/driver_node.h b/packages/livox_driver/src/driver_node.h index b92dedf0d..23f476054 100644 --- a/packages/livox_driver/src/driver_node.h +++ b/packages/livox_driver/src/driver_node.h @@ -31,27 +31,6 @@ namespace livox_ros { class Lddc; -#ifdef BUILDING_ROS1 -class DriverNode final : public ros::NodeHandle { - public: - DriverNode() = default; - DriverNode(const DriverNode&) = delete; - ~DriverNode(); - DriverNode& operator=(const DriverNode&) = delete; - - DriverNode& GetNode() noexcept; - - void PointCloudDataPollThread(); - void ImuDataPollThread(); - - std::unique_ptr lddc_ptr_; - std::shared_ptr pointclouddata_poll_thread_; - std::shared_ptr imudata_poll_thread_; - std::shared_future future_; - std::promise exit_signal_; -}; - -#elif defined BUILDING_ROS2 class DriverNode final : public rclcpp::Node { public: explicit DriverNode(const rclcpp::NodeOptions& options); @@ -71,7 +50,6 @@ class DriverNode final : public rclcpp::Node { std::shared_future future_; std::promise exit_signal_; }; -#endif } // namespace livox_ros diff --git a/packages/livox_driver/src/include/ros_headers.h b/packages/livox_driver/src/include/ros_headers.h index 05993089f..477fff042 100644 --- a/packages/livox_driver/src/include/ros_headers.h +++ b/packages/livox_driver/src/include/ros_headers.h @@ -25,10 +25,6 @@ #ifndef ROS_HEADERS_H_ #define ROS_HEADERS_H_ -#ifdef BUILDING_ROS1 -#include "ros1_headers.h" -#elif defined BUILDING_ROS2 #include "ros2_headers.h" -#endif #endif // ROS_HEADERS_H_ diff --git a/packages/livox_driver/src/lddc.cpp b/packages/livox_driver/src/lddc.cpp index 87890cefa..81cfb76a7 100644 --- a/packages/livox_driver/src/lddc.cpp +++ b/packages/livox_driver/src/lddc.cpp @@ -40,28 +40,6 @@ namespace livox_ros { /** Lidar Data Distribute Control--------------------------------------------*/ -#ifdef BUILDING_ROS1 -Lddc::Lddc( - int format, int multi_topic, int data_src, int output_type, double frq, std::string& frame_id, - bool lidar_bag, bool imu_bag) : - transfer_format_(format), - use_multi_topic_(multi_topic), - data_src_(data_src), - output_type_(output_type), - publish_frq_(frq), - frame_id_(frame_id), - enable_lidar_bag_(lidar_bag), - enable_imu_bag_(imu_bag) { - publish_period_ns_ = kNsPerSecond / publish_frq_; - lds_ = nullptr; - memset(private_pub_, 0, sizeof(private_pub_)); - memset(private_imu_pub_, 0, sizeof(private_imu_pub_)); - global_pub_ = nullptr; - global_imu_pub_ = nullptr; - cur_node_ = nullptr; - bag_ = nullptr; -} -#elif defined BUILDING_ROS2 Lddc::Lddc( int format, int multi_topic, int data_src, int output_type, double frq, std::string& frame_id) : transfer_format_(format), @@ -72,39 +50,11 @@ Lddc::Lddc( frame_id_(frame_id) { publish_period_ns_ = kNsPerSecond / publish_frq_; lds_ = nullptr; -#if 0 - bag_ = nullptr; -#endif } -#endif Lddc::~Lddc() { -#ifdef BUILDING_ROS1 - if (global_pub_) { - delete global_pub_; - } - - if (global_imu_pub_) { - delete global_imu_pub_; - } -#endif - PrepareExit(); - -#ifdef BUILDING_ROS1 - for (uint32_t i = 0; i < kMaxSourceLidar; i++) { - if (private_pub_[i]) { - delete private_pub_[i]; - } - } - - for (uint32_t i = 0; i < kMaxSourceLidar; i++) { - if (private_imu_pub_[i]) { - delete private_imu_pub_[i]; - } - } -#endif - std::cout << "lddc destory!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl; + std::cout << "Lddc destory" << std::endl; } int Lddc::RegisterLds(Lds* lds) { @@ -185,14 +135,6 @@ void Lddc::PollingLidarImuData(uint8_t index, LidarDevice* lidar) { } void Lddc::PrepareExit(void) { -#ifdef BUILDING_ROS1 - if (bag_) { - DRIVER_INFO(*cur_node_, "Waiting to save the bag file!"); - bag_->close(); - DRIVER_INFO(*cur_node_, "Save the bag file successfully!"); - bag_ = nullptr; - } -#endif if (lds_) { lds_->PrepareExit(); lds_ = nullptr; @@ -233,7 +175,6 @@ void Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint8_t index) { /* for pcl::pxyzi */ 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, " @@ -241,7 +182,7 @@ void Lddc::PublishPclMsg(LidarDataQueue* queue, uint8_t index) { } first_log = false; return; -#endif + while (!QueueIsEmpty(queue)) { StoragePacket pkg; QueuePop(queue, &pkg); @@ -309,12 +250,7 @@ void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint if (!pkg.points.empty()) { timestamp = pkg.base_time; } - -#ifdef BUILDING_ROS1 - cloud.header.stamp = ros::Time(timestamp / 1000000000.0); -#elif defined BUILDING_ROS2 cloud.header.stamp = rclcpp::Time(timestamp); -#endif std::vector points; for (size_t i = 0; i < pkg.points_num; ++i) { @@ -334,44 +270,23 @@ void Lddc::InitPointcloud2Msg(const StoragePacket& pkg, PointCloud2& cloud, uint void Lddc::PublishPointcloud2Data( const uint8_t index, const uint64_t timestamp, const PointCloud2& cloud) { -#ifdef BUILDING_ROS1 - PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); -#elif defined BUILDING_ROS2 Publisher::SharedPtr publisher_ptr = std::dynamic_pointer_cast >(GetCurrentPublisher(index)); -#endif if (kOutputToRos == output_type_) { publisher_ptr->publish(cloud); - } else { -#ifdef BUILDING_ROS1 - if (bag_ && enable_lidar_bag_) { - bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud); - } -#endif } } void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index) { livox_msg.header.frame_id.assign(frame_id_); -#ifdef BUILDING_ROS1 - static uint32_t msg_seq = 0; - livox_msg.header.seq = msg_seq; - ++msg_seq; -#endif - uint64_t timestamp = 0; if (!pkg.points.empty()) { timestamp = pkg.base_time; } livox_msg.timebase = timestamp; - -#ifdef BUILDING_ROS1 - livox_msg.header.stamp = ros::Time(timestamp / 1000000000.0); -#elif defined BUILDING_ROS2 livox_msg.header.stamp = rclcpp::Time(timestamp); -#endif livox_msg.point_num = pkg.points_num; if (lds_->lidars_[index].lidar_type == kLivoxLidarType) { @@ -400,80 +315,29 @@ void Lddc::FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg) } void Lddc::PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index) { -#ifdef BUILDING_ROS1 - PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); -#elif defined BUILDING_ROS2 Publisher::SharedPtr publisher_ptr = std::dynamic_pointer_cast >(GetCurrentPublisher(index)); -#endif if (kOutputToRos == output_type_) { publisher_ptr->publish(livox_msg); - } else { -#ifdef BUILDING_ROS1 - if (bag_ && enable_lidar_bag_) { - bag_->write( - publisher_ptr->getTopic(), ros::Time(livox_msg.timebase / 1000000000.0), livox_msg); - } -#endif } } void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp) { -#ifdef BUILDING_ROS1 - cloud.header.frame_id.assign(frame_id_); - cloud.height = 1; - cloud.width = pkg.points_num; - - if (!pkg.points.empty()) { - timestamp = pkg.base_time; - } - 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; -#endif return; } void Lddc::FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg) { -#ifdef BUILDING_ROS1 - if (pkg.points.empty()) { - return; - } - - uint32_t points_num = pkg.points_num; - const std::vector& points = pkg.points; - for (uint32_t i = 0; i < points_num; ++i) { - pcl::PointXYZI point; - point.x = points[i].x; - point.y = points[i].y; - point.z = points[i].z; - point.intensity = points[i].intensity; - - 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; -#endif return; } void Lddc::PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud) { -#ifdef BUILDING_ROS1 - PublisherPtr publisher_ptr = Lddc::GetCurrentPublisher(index); - if (kOutputToRos == output_type_) { - publisher_ptr->publish(cloud); - } else { - if (bag_ && enable_lidar_bag_) { - bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), cloud); - } - } -#elif defined BUILDING_ROS2 std::cout << "warning: pcl::PointCloud is not supported in ROS2, " << "please check code logic" << std::endl; -#endif return; } @@ -481,11 +345,7 @@ void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timest imu_msg.header.frame_id = "livox_frame"; timestamp = imu_data.time_stamp; -#ifdef BUILDING_ROS1 - imu_msg.header.stamp = ros::Time(timestamp / 1000000000.0); // to ros time stamp -#elif defined BUILDING_ROS2 imu_msg.header.stamp = rclcpp::Time(timestamp); // to ros time stamp -#endif imu_msg.angular_velocity.x = imu_data.gyro_x; imu_msg.angular_velocity.y = imu_data.gyro_y; @@ -506,25 +366,14 @@ void Lddc::PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index uint64_t timestamp; InitImuMsg(imu_data, imu_msg, timestamp); -#ifdef BUILDING_ROS1 - PublisherPtr publisher_ptr = GetCurrentImuPublisher(index); -#elif defined BUILDING_ROS2 Publisher::SharedPtr publisher_ptr = std::dynamic_pointer_cast >(GetCurrentImuPublisher(index)); -#endif if (kOutputToRos == output_type_) { publisher_ptr->publish(imu_msg); - } else { -#ifdef BUILDING_ROS1 - if (bag_ && enable_imu_bag_) { - bag_->write(publisher_ptr->getTopic(), ros::Time(timestamp / 1000000000.0), imu_msg); - } -#endif } } -#ifdef BUILDING_ROS2 std::shared_ptr Lddc::CreatePublisher( uint8_t msg_type, std::string& topic_name, uint32_t queue_size) { if (kPointCloud2Msg == msg_type) { @@ -533,15 +382,7 @@ std::shared_ptr Lddc::CreatePublisher( } else if (kLivoxCustomMsg == msg_type) { DRIVER_INFO(*cur_node_, "%s publish use livox custom format", topic_name.c_str()); return cur_node_->create_publisher(topic_name, queue_size); - } -#if 0 - else if (kPclPxyziMsg == msg_type) { - DRIVER_INFO(*cur_node_, - "%s publish use pcl PointXYZI format", topic_name.c_str()); - return cur_node_->create_publisher(topic_name, queue_size); - } -#endif - else if (kLivoxImuMsg == msg_type) { + } else if (kLivoxImuMsg == msg_type) { DRIVER_INFO(*cur_node_, "%s publish use imu format", topic_name.c_str()); return cur_node_->create_publisher(topic_name, queue_size); } else { @@ -549,106 +390,7 @@ std::shared_ptr Lddc::CreatePublisher( return null_publisher; } } -#endif - -#ifdef BUILDING_ROS1 -PublisherPtr Lddc::GetCurrentPublisher(uint8_t index) { - ros::Publisher** pub = nullptr; - uint32_t queue_size = kMinEthPacketQueueSize; - if (use_multi_topic_) { - pub = &private_pub_[index]; - queue_size = queue_size / 8; // queue size is 4 for only one lidar - } else { - pub = &global_pub_; - queue_size = queue_size * 8; // shared queue size is 256, for all lidars - } - - if (*pub == nullptr) { - char name_str[48]; - memset(name_str, 0, sizeof(name_str)); - if (use_multi_topic_) { - std::string ip_string = IpNumToString(lds_->lidars_[index].handle); - snprintf( - name_str, - sizeof(name_str), - "livox/lidar_%s", - ReplacePeriodByUnderline(ip_string).c_str()); - DRIVER_INFO(*cur_node_, "Support multi topics."); - } else { - DRIVER_INFO(*cur_node_, "Support only one topic."); - snprintf(name_str, sizeof(name_str), "livox/lidar"); - } - - *pub = new ros::Publisher; - if (kPointCloud2Msg == transfer_format_) { - **pub = cur_node_->GetNode().advertise(name_str, queue_size); - DRIVER_INFO( - *cur_node_, - "%s publish use PointCloud2 format, set ROS publisher queue size %d", - name_str, - queue_size); - } else if (kLivoxCustomMsg == transfer_format_) { - **pub = cur_node_->GetNode().advertise(name_str, queue_size); - DRIVER_INFO( - *cur_node_, - "%s publish use livox custom format, set ROS publisher queue size %d", - name_str, - queue_size); - } else if (kPclPxyziMsg == transfer_format_) { - **pub = cur_node_->GetNode().advertise(name_str, queue_size); - DRIVER_INFO( - *cur_node_, - "%s publish use pcl PointXYZI format, set ROS publisher queue " - "size %d", - name_str, - queue_size); - } - } - - return *pub; -} - -PublisherPtr Lddc::GetCurrentImuPublisher(uint8_t handle) { - ros::Publisher** pub = nullptr; - uint32_t queue_size = kMinEthPacketQueueSize; - - if (use_multi_topic_) { - pub = &private_imu_pub_[handle]; - queue_size = queue_size * 2; // queue size is 64 for only one lidar - } else { - pub = &global_imu_pub_; - queue_size = queue_size * 8; // shared queue size is 256, for all lidars - } - - if (*pub == nullptr) { - char name_str[48]; - memset(name_str, 0, sizeof(name_str)); - if (use_multi_topic_) { - DRIVER_INFO(*cur_node_, "Support multi topics."); - std::string ip_string = IpNumToString(lds_->lidars_[handle].handle); - snprintf( - name_str, - sizeof(name_str), - "livox/imu_%s", - ReplacePeriodByUnderline(ip_string).c_str()); - } else { - DRIVER_INFO(*cur_node_, "Support only one topic."); - snprintf(name_str, sizeof(name_str), "livox/imu"); - } - - *pub = new ros::Publisher; - **pub = cur_node_->GetNode().advertise(name_str, queue_size); - DRIVER_INFO( - *cur_node_, - "%s publish imu data, set ROS publisher queue size %d", - name_str, - queue_size); - } - - return *pub; -} -#elif defined BUILDING_ROS2 std::shared_ptr Lddc::GetCurrentPublisher(uint8_t handle) { uint32_t queue_size = kMinEthPacketQueueSize; if (use_multi_topic_) { @@ -703,16 +445,7 @@ std::shared_ptr Lddc::GetCurrentImuPublisher(uint8_t hand return global_imu_pub_; } } -#endif - -void Lddc::CreateBagFile(const std::string& file_name) { -#ifdef BUILDING_ROS1 - if (!bag_) { - bag_ = new rosbag::Bag; - bag_->open(file_name, rosbag::bagmode::Write); - DRIVER_INFO(*cur_node_, "Create bag file :%s!", file_name.c_str()); - } -#endif -} + +void Lddc::CreateBagFile(const std::string& file_name) {} } // namespace livox_ros diff --git a/packages/livox_driver/src/lddc.h b/packages/livox_driver/src/lddc.h index 9ce5f91ef..0af70b362 100644 --- a/packages/livox_driver/src/lddc.h +++ b/packages/livox_driver/src/lddc.h @@ -47,15 +47,6 @@ typedef enum { } TransferType; /** Type-Definitions based on ROS versions */ -#ifdef BUILDING_ROS1 -using Publisher = ros::Publisher; -using PublisherPtr = ros::Publisher*; -using PointCloud2 = sensor_msgs::PointCloud2; -using PointField = sensor_msgs::PointField; -using CustomMsg = livox_driver::CustomMsg; -using CustomPoint = livox_driver::CustomPoint; -using ImuMsg = sensor_msgs::Imu; -#elif defined BUILDING_ROS2 template using Publisher = rclcpp::Publisher; using PublisherPtr = std::shared_ptr; @@ -64,7 +55,6 @@ using PointField = sensor_msgs::msg::PointField; using CustomMsg = livox_driver::msg::CustomMsg; using CustomPoint = livox_driver::msg::CustomPoint; using ImuMsg = sensor_msgs::msg::Imu; -#endif using PointCloud = pcl::PointCloud; @@ -72,15 +62,9 @@ class DriverNode; class Lddc final { public: -#ifdef BUILDING_ROS1 - Lddc( - int format, int multi_topic, int data_src, int output_type, double frq, - std::string& frame_id, bool lidar_bag, bool imu_bag); -#elif defined BUILDING_ROS2 Lddc( int format, int multi_topic, int data_src, int output_type, double frq, std::string& frame_id); -#endif ~Lddc(); int RegisterLds(Lds* lds); @@ -128,10 +112,7 @@ class Lddc final { CustomMsg& livox_msg, LivoxPointXyzrtlt* src_point, uint32_t num, uint32_t offset_time, uint32_t point_interval, uint32_t echo_num); -#ifdef BUILDING_ROS2 PublisherPtr CreatePublisher(uint8_t msg_type, std::string& topic_name, uint32_t queue_size); -#endif - PublisherPtr GetCurrentPublisher(uint8_t index); PublisherPtr GetCurrentImuPublisher(uint8_t index); @@ -144,20 +125,10 @@ class Lddc final { uint32_t publish_period_ns_; std::string frame_id_; -#ifdef BUILDING_ROS1 - bool enable_lidar_bag_; - bool enable_imu_bag_; - PublisherPtr private_pub_[kMaxSourceLidar]; - PublisherPtr global_pub_; - PublisherPtr private_imu_pub_[kMaxSourceLidar]; - PublisherPtr global_imu_pub_; - rosbag::Bag* bag_; -#elif defined BUILDING_ROS2 PublisherPtr private_pub_[kMaxSourceLidar]; PublisherPtr global_pub_; PublisherPtr private_imu_pub_[kMaxSourceLidar]; PublisherPtr global_imu_pub_; -#endif livox_ros::DriverNode* cur_node_; }; diff --git a/packages/livox_driver/src/lds_lidar.cpp b/packages/livox_driver/src/lds_lidar.cpp index d6ff8ee69..09281fd18 100644 --- a/packages/livox_driver/src/lds_lidar.cpp +++ b/packages/livox_driver/src/lds_lidar.cpp @@ -125,9 +125,7 @@ bool LdsLidar::ParseSummaryConfig() { } bool LdsLidar::InitLivoxLidar() { -#ifdef BUILDING_ROS2 DisableLivoxSdkConsoleLogger(); -#endif // parse user config LivoxLidarConfigParser parser(path_); @@ -198,6 +196,13 @@ int LdsLidar::DeInitLdsLidar(void) { return -1; } + // Assuming there's only one lidar so checking index 0 only + // https://github.com/Livox-SDK/livox_ros_driver2/issues/103#issuecomment-1899871800 + if (lidars_[0].connect_state == kConnectStateSampling) { + printf("Shutting down lidar (handle: %d)\n", lidars_[0].handle); + SetLivoxLidarWorkMode(lidars_[0].handle, kLivoxLidarWakeUp, nullptr, nullptr); + } + if (lidar_summary_info_.lidar_type & kLivoxLidarType) { LivoxLidarSdkUninit(); printf("Livox Lidar SDK Deinit completely!\n"); diff --git a/packages/livox_driver/src/livox_driver.cpp b/packages/livox_driver/src/livox_driver.cpp index 9f3028190..e2cc6c601 100644 --- a/packages/livox_driver/src/livox_driver.cpp +++ b/packages/livox_driver/src/livox_driver.cpp @@ -36,94 +36,6 @@ using namespace livox_ros; -#ifdef BUILDING_ROS1 -int main(int argc, char** argv) { - /** Ros related */ - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { - ros::console::notifyLoggerLevelsChanged(); - } - - ros::init(argc, argv, "livox_lidar_publisher"); - - // ros::NodeHandle livox_node; - livox_ros::DriverNode livox_node; - - DRIVER_INFO(livox_node, "Livox Ros Driver2 Version: %s", LIVOX_ROS_DRIVER2_VERSION_STRING); - - /** Init default system parameter */ - int xfer_format = kPointCloud2Msg; - int multi_topic = 0; - int data_src = kSourceRawLidar; - double publish_freq = 10.0; /* Hz */ - int output_type = kOutputToRos; - std::string frame_id = "livox_frame"; - bool lidar_bag = true; - bool imu_bag = false; - - livox_node.GetNode().getParam("xfer_format", xfer_format); - livox_node.GetNode().getParam("multi_topic", multi_topic); - livox_node.GetNode().getParam("data_src", data_src); - livox_node.GetNode().getParam("publish_freq", publish_freq); - livox_node.GetNode().getParam("output_data_type", output_type); - livox_node.GetNode().getParam("frame_id", frame_id); - livox_node.GetNode().getParam("enable_lidar_bag", lidar_bag); - livox_node.GetNode().getParam("enable_imu_bag", imu_bag); - - printf("data source:%u.\n", data_src); - - if (publish_freq > 100.0) { - publish_freq = 100.0; - } else if (publish_freq < 0.5) { - publish_freq = 0.5; - } else { - publish_freq = publish_freq; - } - - livox_node.future_ = livox_node.exit_signal_.get_future(); - - /** Lidar data distribute control and lidar data source set */ - livox_node.lddc_ptr_ = std::make_unique( - xfer_format, - multi_topic, - data_src, - output_type, - publish_freq, - frame_id, - lidar_bag, - imu_bag); - livox_node.lddc_ptr_->SetRosNode(&livox_node); - - if (data_src == kSourceRawLidar) { - DRIVER_INFO(livox_node, "Data Source is raw lidar."); - - std::string user_config_path; - livox_node.getParam("user_config_path", user_config_path); - DRIVER_INFO(livox_node, "Config file : %s", user_config_path.c_str()); - - LdsLidar* read_lidar = LdsLidar::GetInstance(publish_freq); - livox_node.lddc_ptr_->RegisterLds(static_cast(read_lidar)); - - if ((read_lidar->InitLdsLidar(user_config_path))) { - DRIVER_INFO(livox_node, "Init lds lidar successfully!"); - } else { - DRIVER_ERROR(livox_node, "Init lds lidar failed!"); - } - } else { - DRIVER_ERROR(livox_node, "Invalid data src (%d), please check the launch file", data_src); - } - - livox_node.pointclouddata_poll_thread_ = - std::make_shared(&DriverNode::PointCloudDataPollThread, &livox_node); - livox_node.imudata_poll_thread_ = - std::make_shared(&DriverNode::ImuDataPollThread, &livox_node); - while (ros::ok()) { - usleep(10000); - } - - return 0; -} - -#elif defined BUILDING_ROS2 namespace livox_ros { DriverNode::DriverNode(const rclcpp::NodeOptions& node_options) : Node("livox_driver_node", node_options) { @@ -144,8 +56,6 @@ DriverNode::DriverNode(const rclcpp::NodeOptions& node_options) : this->declare_parameter("output_data_type", output_type); this->declare_parameter("frame_id", "frame_default"); 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->get_parameter("xfer_format", xfer_format); this->get_parameter("multi_topic", multi_topic); @@ -158,8 +68,6 @@ DriverNode::DriverNode(const rclcpp::NodeOptions& node_options) : publish_freq = 100.0; } else if (publish_freq < 0.5) { publish_freq = 0.5; - } else { - publish_freq = publish_freq; } future_ = exit_signal_.get_future(); @@ -176,9 +84,6 @@ DriverNode::DriverNode(const rclcpp::NodeOptions& node_options) : this->get_parameter("user_config_path", user_config_path); DRIVER_INFO(*this, "Config file : %s", user_config_path.c_str()); - std::string cmdline_bd_code; - this->get_parameter("cmdline_input_bd_code", cmdline_bd_code); - LdsLidar* read_lidar = LdsLidar::GetInstance(publish_freq); lddc_ptr_->RegisterLds(static_cast(read_lidar)); @@ -201,8 +106,6 @@ DriverNode::DriverNode(const rclcpp::NodeOptions& node_options) : #include RCLCPP_COMPONENTS_REGISTER_NODE(livox_ros::DriverNode) -#endif // defined BUILDING_ROS2 - void DriverNode::PointCloudDataPollThread() { std::future_status status; std::this_thread::sleep_for(std::chrono::seconds(3)); diff --git a/packages/truck/launch/lidar3d.yaml b/packages/truck/launch/lidar3d.yaml new file mode 100644 index 000000000..1e9aa1d3d --- /dev/null +++ b/packages/truck/launch/lidar3d.yaml @@ -0,0 +1,8 @@ +- include: + file: $(find-pkg-share livox_driver)/launch/livox_driver.py + arg: + - { name: "lidar_ip", value: "192.168.2.10" } + - { name: "host_iface", value: "eth1" } + - { name: "frame_id", value: "lidar_link" } + - { name: "publish_freq", value: "10" } + - { name: "scan_pattern_mode", value: "0" } diff --git a/packages/truck/launch/truck.yaml b/packages/truck/launch/truck.yaml index e4e3ea335..4b89b4922 100644 --- a/packages/truck/launch/truck.yaml +++ b/packages/truck/launch/truck.yaml @@ -1,6 +1,6 @@ launch: - include: - file: $(find-pkg-share truck)/launch/lidar.yaml + file: $(find-pkg-share truck)/launch/lidar3d.yaml - include: file: $(find-pkg-share truck)/launch/camera.yaml From 3b55f3bc2f3f6f84c148069e1b5ca1efb8e03b66 Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sun, 24 Nov 2024 05:13:44 +0000 Subject: [PATCH 04/10] Remove PCL dependency --- packages/livox_driver/CMakeLists.txt | 19 ++----- packages/livox_driver/package.xml | 2 - .../livox_driver/src/include/ros1_headers.h | 53 ------------------- .../livox_driver/src/include/ros2_headers.h | 52 ------------------ .../livox_driver/src/include/ros_headers.h | 29 ++++++++-- packages/livox_driver/src/lddc.cpp | 47 ---------------- packages/livox_driver/src/lddc.h | 8 --- 7 files changed, 29 insertions(+), 181 deletions(-) delete mode 100644 packages/livox_driver/src/include/ros1_headers.h delete mode 100644 packages/livox_driver/src/include/ros2_headers.h diff --git a/packages/livox_driver/CMakeLists.txt b/packages/livox_driver/CMakeLists.txt index 7fb95e236..9a6f358e5 100644 --- a/packages/livox_driver/CMakeLists.txt +++ b/packages/livox_driver/CMakeLists.txt @@ -13,8 +13,6 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() -list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules") - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) endif() @@ -36,7 +34,6 @@ message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}") # dependencies manually. find_package( REQUIRED) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(PCL REQUIRED) find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -73,10 +70,6 @@ find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR NAMES "livox_lidar_api.h" "livox_lidar_def.h" REQUIRED ) -# PCL library -link_directories(${PCL_LIBRARY_DIRS}) -add_definitions(${PCL_DEFINITIONS}) - # livox ros2 driver target ament_auto_add_library( ${PROJECT_NAME} @@ -116,20 +109,16 @@ target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}") # include file direcotry target_include_directories( ${PROJECT_NAME} - PUBLIC ${PCL_INCLUDE_DIRS} ${APR_INCLUDE_DIRS} ${LIVOX_LIDAR_SDK_INCLUDE_DIR} + PUBLIC ${APR_INCLUDE_DIRS} ${LIVOX_LIDAR_SDK_INCLUDE_DIR} ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs src ) # link libraries target_link_libraries( - ${PROJECT_NAME} - ${LIVOX_LIDAR_SDK_LIBRARY} + ${PROJECT_NAME} ${LIVOX_LIDAR_SDK_LIBRARY} ${LIVOX_INTERFACE_TARGET} # for custom msgs - ${PPT_LIBRARY} - ${Boost_LIBRARY} - ${PCL_LIBRARIES} - ${APR_LIBRARIES} + ${PPT_LIBRARY} ${Boost_LIBRARY} ${APR_LIBRARIES} ) rclcpp_components_register_node( @@ -147,4 +136,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package(INSTALL_TO_SHARE config launch) +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/packages/livox_driver/package.xml b/packages/livox_driver/package.xml index 9c3d7b525..641f8b112 100644 --- a/packages/livox_driver/package.xml +++ b/packages/livox_driver/package.xml @@ -16,9 +16,7 @@ std_msgs sensor_msgs rcutils - pcl_conversions rcl_interfaces - libpcl-all-dev rosbag2 rosidl_default_runtime diff --git a/packages/livox_driver/src/include/ros1_headers.h b/packages/livox_driver/src/include/ros1_headers.h deleted file mode 100644 index a1012c801..000000000 --- a/packages/livox_driver/src/include/ros1_headers.h +++ /dev/null @@ -1,53 +0,0 @@ -// -// The MIT License (MIT) -// -// Copyright (c) 2022 Livox. All rights reserved. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -// - -// Denoting headers specifically used for building ROS1 Driver. - -#ifndef ROS1_HEADERS_H_ -#define ROS1_HEADERS_H_ - -#include -#include - -#include -#include -#include -#include -#include -#include "livox_driver/CustomMsg.h" -#include "livox_driver/CustomPoint.h" - -#define DRIVER_DEBUG(node, ...) ROS_DEBUG(__VA_ARGS__) -#define DRIVER_INFO(node, ...) ROS_INFO(__VA_ARGS__) -#define DRIVER_WARN(node, ...) ROS_WARN(__VA_ARGS__) -#define DRIVER_ERROR(node, ...) ROS_ERROR(__VA_ARGS__) -#define DRIVER_FATAL(node, ...) ROS_FATAL(__VA_ARGS__) - -#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) ROS_DEBUG_##EXTRA(__VA_ARGS__) -#define DRIVER_INFO_EXTRA(node, EXTRA, ...) ROS_INFO_##EXTRA(__VA_ARGS__) -#define DRIVER_WARN_EXTRA(node, EXTRA, ...) ROS_WARN_##EXTRA(__VA_ARGS__) -#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) ROS_ERROR_##EXTRA(__VA_ARGS__) -#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) ROS_FATAL_##EXTRA(__VA_ARGS__) - -#endif // ROS1_HEADERS_H_ diff --git a/packages/livox_driver/src/include/ros2_headers.h b/packages/livox_driver/src/include/ros2_headers.h deleted file mode 100644 index 069b40323..000000000 --- a/packages/livox_driver/src/include/ros2_headers.h +++ /dev/null @@ -1,52 +0,0 @@ -// -// The MIT License (MIT) -// -// Copyright (c) 2022 Livox. All rights reserved. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -// - -// Denoting headers specifically used for building ROS1 Driver. - -#ifndef ROS2_HEADERS_H_ -#define ROS2_HEADERS_H_ - -#include -#include - -#include -#include -#include -#include -#include "livox_driver/msg/custom_point.hpp" -#include "livox_driver/msg/custom_msg.hpp" - -#define DRIVER_DEBUG(node, ...) RCLCPP_DEBUG((node).get_logger(), __VA_ARGS__) -#define DRIVER_INFO(node, ...) RCLCPP_INFO((node).get_logger(), __VA_ARGS__) -#define DRIVER_WARN(node, ...) RCLCPP_WARN((node).get_logger(), __VA_ARGS__) -#define DRIVER_ERROR(node, ...) RCLCPP_ERROR((node).get_logger(), __VA_ARGS__) -#define DRIVER_FATAL(node, ...) RCLCPP_FATAL((node).get_logger(), __VA_ARGS__) - -#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) RCLCPP_DEBUG_##EXTRA((node).get_logger(), __VA_ARGS__) -#define DRIVER_INFO_EXTRA(node, EXTRA, ...) RCLCPP_INFO_##EXTRA((node).get_logger(), __VA_ARGS__) -#define DRIVER_WARN_EXTRA(node, EXTRA, ...) RCLCPP_WARN_##EXTRA((node).get_logger(), __VA_ARGS__) -#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) RCLCPP_ERROR_##EXTRA((node).get_logger(), __VA_ARGS__) -#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) RCLCPP_FATAL_##EXTRA((node).get_logger(), __VA_ARGS__) - -#endif // ROS2_HEADERS_H_ diff --git a/packages/livox_driver/src/include/ros_headers.h b/packages/livox_driver/src/include/ros_headers.h index 477fff042..b43e52150 100644 --- a/packages/livox_driver/src/include/ros_headers.h +++ b/packages/livox_driver/src/include/ros_headers.h @@ -22,9 +22,30 @@ // SOFTWARE. // -#ifndef ROS_HEADERS_H_ -#define ROS_HEADERS_H_ +// Denoting headers specifically used for building ROS1 Driver. -#include "ros2_headers.h" +#ifndef ROS2_HEADERS_H_ +#define ROS2_HEADERS_H_ -#endif // ROS_HEADERS_H_ +#include +#include + +#include +#include +#include +#include "livox_driver/msg/custom_point.hpp" +#include "livox_driver/msg/custom_msg.hpp" + +#define DRIVER_DEBUG(node, ...) RCLCPP_DEBUG((node).get_logger(), __VA_ARGS__) +#define DRIVER_INFO(node, ...) RCLCPP_INFO((node).get_logger(), __VA_ARGS__) +#define DRIVER_WARN(node, ...) RCLCPP_WARN((node).get_logger(), __VA_ARGS__) +#define DRIVER_ERROR(node, ...) RCLCPP_ERROR((node).get_logger(), __VA_ARGS__) +#define DRIVER_FATAL(node, ...) RCLCPP_FATAL((node).get_logger(), __VA_ARGS__) + +#define DRIVER_DEBUG_EXTRA(node, EXTRA, ...) RCLCPP_DEBUG_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_INFO_EXTRA(node, EXTRA, ...) RCLCPP_INFO_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_WARN_EXTRA(node, EXTRA, ...) RCLCPP_WARN_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_ERROR_EXTRA(node, EXTRA, ...) RCLCPP_ERROR_##EXTRA((node).get_logger(), __VA_ARGS__) +#define DRIVER_FATAL_EXTRA(node, EXTRA, ...) RCLCPP_FATAL_##EXTRA((node).get_logger(), __VA_ARGS__) + +#endif // ROS2_HEADERS_H_ diff --git a/packages/livox_driver/src/lddc.cpp b/packages/livox_driver/src/lddc.cpp index 81cfb76a7..cd2ad4bc8 100644 --- a/packages/livox_driver/src/lddc.cpp +++ b/packages/livox_driver/src/lddc.cpp @@ -121,8 +121,6 @@ void Lddc::PollingLidarPointCloudData(uint8_t index, LidarDevice* lidar) { PublishPointcloud2(p_queue, index); } else if (kLivoxCustomMsg == transfer_format_) { PublishCustomPointcloud(p_queue, index); - } else if (kPclPxyziMsg == transfer_format_) { - PublishPclMsg(p_queue, index); } } } @@ -173,33 +171,6 @@ void Lddc::PublishCustomPointcloud(LidarDataQueue* queue, uint8_t index) { } } -/* for pcl::pxyzi */ -void Lddc::PublishPclMsg(LidarDataQueue* queue, uint8_t index) { - 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; - return; - - while (!QueueIsEmpty(queue)) { - StoragePacket pkg; - QueuePop(queue, &pkg); - if (pkg.points.empty()) { - printf("Publish point cloud failed, the pkg points is empty.\n"); - continue; - } - - PointCloud cloud; - uint64_t timestamp = 0; - InitPclMsg(pkg, cloud, timestamp); - FillPointsToPclMsg(pkg, cloud); - PublishPclData(index, timestamp, cloud); - } - return; -} - void Lddc::InitPointcloud2MsgHeader(PointCloud2& cloud) { cloud.header.frame_id.assign(frame_id_); cloud.height = 1; @@ -323,24 +294,6 @@ void Lddc::PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t inde } } -void Lddc::InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp) { - std::cout << "warning: pcl::PointCloud is not supported in ROS2, " - << "please check code logic" << std::endl; - return; -} - -void Lddc::FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg) { - std::cout << "warning: pcl::PointCloud is not supported in ROS2, " - << "please check code logic" << std::endl; - return; -} - -void Lddc::PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud) { - std::cout << "warning: pcl::PointCloud is not supported in ROS2, " - << "please check code logic" << std::endl; - return; -} - void Lddc::InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp) { imu_msg.header.frame_id = "livox_frame"; diff --git a/packages/livox_driver/src/lddc.h b/packages/livox_driver/src/lddc.h index 0af70b362..813d8f4ad 100644 --- a/packages/livox_driver/src/lddc.h +++ b/packages/livox_driver/src/lddc.h @@ -56,8 +56,6 @@ using CustomMsg = livox_driver::msg::CustomMsg; using CustomPoint = livox_driver::msg::CustomPoint; using ImuMsg = sensor_msgs::msg::Imu; -using PointCloud = pcl::PointCloud; - class DriverNode; class Lddc final { @@ -89,7 +87,6 @@ class Lddc final { void PublishPointcloud2(LidarDataQueue* queue, uint8_t index); void PublishCustomPointcloud(LidarDataQueue* queue, uint8_t index); - void PublishPclMsg(LidarDataQueue* queue, uint8_t index); void PublishImuData(LidarImuDataQueue& imu_data_queue, const uint8_t index); @@ -101,13 +98,8 @@ class Lddc final { void FillPointsToCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg); void PublishCustomPointData(const CustomMsg& livox_msg, const uint8_t index); - void InitPclMsg(const StoragePacket& pkg, PointCloud& cloud, uint64_t& timestamp); - void FillPointsToPclMsg(const StoragePacket& pkg, PointCloud& pcl_msg); - void PublishPclData(const uint8_t index, const uint64_t timestamp, const PointCloud& cloud); - void InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp); - void FillPointsToPclMsg(PointCloud& pcl_msg, LivoxPointXyzrtlt* src_point, uint32_t num); void FillPointsToCustomMsg( CustomMsg& livox_msg, LivoxPointXyzrtlt* src_point, uint32_t num, uint32_t offset_time, uint32_t point_interval, uint32_t echo_num); From c14524f8dc8a4f0b5149916fe7a264b2fea41413 Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sun, 24 Nov 2024 05:14:25 +0000 Subject: [PATCH 05/10] Fix launch + macvlan config --- lidar-net.yaml | 20 ++++++++++++++++++++ macvlan.yaml | 20 -------------------- packages/truck/launch/lidar3d.yaml | 5 +++-- 3 files changed, 23 insertions(+), 22 deletions(-) create mode 100644 lidar-net.yaml delete mode 100644 macvlan.yaml diff --git a/lidar-net.yaml b/lidar-net.yaml new file mode 100644 index 000000000..ca9e30063 --- /dev/null +++ b/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: + macvlan: + +networks: + macvlan: + 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" diff --git a/macvlan.yaml b/macvlan.yaml deleted file mode 100644 index 5af5014ea..000000000 --- a/macvlan.yaml +++ /dev/null @@ -1,20 +0,0 @@ -# Use together with main docker-compose.yaml file: -# docker compose -f docker-compose.yaml -f macvlan.yaml up -d - -services: - truck: - networks: - default: - macvlan: - -networks: - macvlan: - name: "${CONTAINER_NAME:-truck-${USER}}-macvlan" - driver: macvlan - driver_opts: - parent: eth0 - ipam: - config: - - subnet: 192.168.1.0/24 - gateway: 192.168.1.1 - ip_range: 192.168.1.55/32 # TODO: Use user ID or dynamic address diff --git a/packages/truck/launch/lidar3d.yaml b/packages/truck/launch/lidar3d.yaml index 1e9aa1d3d..1ec564b09 100644 --- a/packages/truck/launch/lidar3d.yaml +++ b/packages/truck/launch/lidar3d.yaml @@ -1,8 +1,9 @@ +launch: - include: file: $(find-pkg-share livox_driver)/launch/livox_driver.py arg: - - { name: "lidar_ip", value: "192.168.2.10" } - - { name: "host_iface", value: "eth1" } - { name: "frame_id", value: "lidar_link" } - { name: "publish_freq", value: "10" } - { name: "scan_pattern_mode", value: "0" } + - { name: "lidar_ip", value: "192.168.2.10" } + - { name: "host_iface", value: "eth1" } From ebe7a2d6e4be364785b25c8d85bfa37489307380 Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sun, 24 Nov 2024 05:19:39 +0000 Subject: [PATCH 06/10] Add rapidjson to dockerfile --- Dockerfile | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/Dockerfile b/Dockerfile index 61af1d8a3..208f8de0a 100644 --- a/Dockerfile +++ b/Dockerfile @@ -537,6 +537,16 @@ RUN wget -qO - https://github.com/Livox-SDK/Livox-SDK2/archive/refs/tags/v${LIVO && 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 From 5f54923311da33b544056fb2ee2b3c32a5046cf4 Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sun, 24 Nov 2024 05:23:26 +0000 Subject: [PATCH 07/10] Remove APR references --- packages/livox_driver/CMakeLists.txt | 12 ++---------- packages/livox_driver/package.xml | 3 --- 2 files changed, 2 insertions(+), 13 deletions(-) diff --git a/packages/livox_driver/CMakeLists.txt b/packages/livox_driver/CMakeLists.txt index 9a6f358e5..3f43ba1aa 100644 --- a/packages/livox_driver/CMakeLists.txt +++ b/packages/livox_driver/CMakeLists.txt @@ -39,14 +39,6 @@ find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(RapidJSON) -# check apr -find_package(PkgConfig) -pkg_check_modules(APR apr-1) -if(APR_FOUND) - message(${APR_INCLUDE_DIRS}) - message(${APR_LIBRARIES}) -endif(APR_FOUND) - # generate custom msg headers set(LIVOX_INTERFACES livox_interfaces2) rosidl_generate_interfaces( @@ -109,7 +101,7 @@ target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}") # include file direcotry target_include_directories( ${PROJECT_NAME} - PUBLIC ${APR_INCLUDE_DIRS} ${LIVOX_LIDAR_SDK_INCLUDE_DIR} + PUBLIC ${LIVOX_LIDAR_SDK_INCLUDE_DIR} ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs src ) @@ -118,7 +110,7 @@ target_include_directories( target_link_libraries( ${PROJECT_NAME} ${LIVOX_LIDAR_SDK_LIBRARY} ${LIVOX_INTERFACE_TARGET} # for custom msgs - ${PPT_LIBRARY} ${Boost_LIBRARY} ${APR_LIBRARIES} + ${PPT_LIBRARY} ${Boost_LIBRARY} ) rclcpp_components_register_node( diff --git a/packages/livox_driver/package.xml b/packages/livox_driver/package.xml index 641f8b112..7fd8aeb86 100644 --- a/packages/livox_driver/package.xml +++ b/packages/livox_driver/package.xml @@ -24,9 +24,6 @@ ament_lint_auto ament_lint_common - git - apr - ament_cmake From c7eb7e0138fdc78053c2d22d13b9328b22619ba3 Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sun, 24 Nov 2024 05:24:56 +0000 Subject: [PATCH 08/10] Rename network --- lidar-net.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lidar-net.yaml b/lidar-net.yaml index ca9e30063..82fae012a 100644 --- a/lidar-net.yaml +++ b/lidar-net.yaml @@ -5,10 +5,10 @@ services: truck: networks: default: - macvlan: + lidar-net: networks: - macvlan: + lidar-net: name: "${CONTAINER_NAME:-truck-${USER}}-lidar-net" driver: macvlan driver_opts: From 75d18c0fac706df632cd97759c7b4d6982dcc308 Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sun, 2 Feb 2025 13:34:01 +0000 Subject: [PATCH 09/10] Fix lidar rotation --- packages/model/config/model.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packages/model/config/model.yaml b/packages/model/config/model.yaml index c60e9bc5d..8bd2bde36 100644 --- a/packages/model/config/model.yaml +++ b/packages/model/config/model.yaml @@ -36,7 +36,7 @@ tf_static: - frame_id: "base" child_frame_id: "lidar_link" translation: { x: 0.0, y: 0.0, z: 0.0 } - rotation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } + rotation: { x: 0, y: 0.0, z: 1.0, w: 0.0 } - frame_id: "base" child_frame_id: "camera_link" From 8dc22a77196cd28b8d2f20f09a797fddecc1ce08 Mon Sep 17 00:00:00 2001 From: Andrew Bond Date: Sun, 2 Feb 2025 13:59:16 +0000 Subject: [PATCH 10/10] Fix number format --- packages/model/config/model.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/packages/model/config/model.yaml b/packages/model/config/model.yaml index 8bd2bde36..01ccaf135 100644 --- a/packages/model/config/model.yaml +++ b/packages/model/config/model.yaml @@ -36,7 +36,7 @@ tf_static: - frame_id: "base" child_frame_id: "lidar_link" translation: { x: 0.0, y: 0.0, z: 0.0 } - rotation: { x: 0, y: 0.0, z: 1.0, w: 0.0 } + rotation: { x: 0.0, y: 0.0, z: 1.0, w: 0.0 } - frame_id: "base" child_frame_id: "camera_link"