diff --git a/Dockerfile b/Dockerfile index dd7e4a07d..208f8de0a 100644 --- a/Dockerfile +++ b/Dockerfile @@ -528,6 +528,25 @@ RUN wget -qO - https://github.com/borglab/gtsam/archive/refs/tags/${GTSAM_VERSIO && make -j$(nproc) install \ && rm -rf /tmp/* +### INSTALL LIVOX SDK2 + +ARG LIVOX_VERSION="1.2.5" + +RUN wget -qO - https://github.com/Livox-SDK/Livox-SDK2/archive/refs/tags/v${LIVOX_VERSION}.tar.gz | tar -xz \ + && cd Livox-SDK2-${LIVOX_VERSION} && mkdir build && cd build \ + && cmake .. && make -j$(nproc) && make install \ + && rm -rf /tmp/* + +### INSTALL RAPIDJSON + +ARG RAPIDJSON_VERSION="1.1.0" + +RUN wget -qO - https://github.com/Tencent/rapidjson/archive/refs/tags/v${RAPIDJSON_VERSION}.tar.gz | tar -xz \ + && cd rapidjson-${RAPIDJSON_VERSION} && mkdir build && cd build \ + && cmake .. -DRAPIDJSON_BUILD_DOC=OFF -DRAPIDJSON_BUILD_EXAMPLES=OFF -DRAPIDJSON_BUILD_TESTS=OFF \ + && make -j$(nproc) && make install \ + && rm -rf /tmp/* + ### INSTALL DEV PKGS COPY requirements.txt /tmp/requirements.txt diff --git a/lidar-net.yaml b/lidar-net.yaml new file mode 100644 index 000000000..82fae012a --- /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: + lidar-net: + +networks: + lidar-net: + name: "${CONTAINER_NAME:-truck-${USER}}-lidar-net" + driver: macvlan + driver_opts: + parent: eth0 + ipam: + config: + - subnet: 192.168.2.0/24 + gateway: 192.168.2.1 + ip_range: "192.168.2.1${PORT_SUFFIX:-00}/32" diff --git a/packages/livox_driver/CMakeLists.txt b/packages/livox_driver/CMakeLists.txt new file mode 100644 index 000000000..3f43ba1aa --- /dev/null +++ b/packages/livox_driver/CMakeLists.txt @@ -0,0 +1,131 @@ +# Copyright(c) 2020 livoxtech limited. + +cmake_minimum_required(VERSION 3.14) +project(livox_driver) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) +endif() + +# Printf version info +include(cmake/version.cmake) +project( + ${PROJECT_NAME} + VERSION ${LIVOX_ROS_DRIVER2_VERSION} + LANGUAGES CXX +) +message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}") + +# ----------------------------------------------------------------------------- +# Add ROS Version MACRO +# ----------------------------------------------------------------------------- + +# find dependencies uncomment the following section in order to fill in further +# dependencies manually. find_package( REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(RapidJSON) + +# generate custom msg headers +set(LIVOX_INTERFACES livox_interfaces2) +rosidl_generate_interfaces( + ${LIVOX_INTERFACES} + "msg/CustomPoint.msg" + "msg/CustomMsg.msg" + DEPENDENCIES + builtin_interfaces + std_msgs + LIBRARY_NAME + ${PROJECT_NAME} +) + +# make sure the livox_lidar_sdk_shared library is installed +find_library( + LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED +) + +# +find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR NAMES "livox_lidar_api.h" + "livox_lidar_def.h" REQUIRED +) + +# livox ros2 driver target +ament_auto_add_library( + ${PROJECT_NAME} + SHARED + src/livox_driver.cpp + src/lddc.cpp + src/driver_node.cpp + src/lds.cpp + src/lds_lidar.cpp + src/comm/comm.cpp + src/comm/ldq.cpp + src/comm/semaphore.cpp + src/comm/lidar_imu_data_queue.cpp + src/comm/cache_index.cpp + src/comm/pub_handler.cpp + src/parse_cfg_file/parse_cfg_file.cpp + src/parse_cfg_file/parse_livox_lidar_cfg.cpp + src/call_back/lidar_common_callback.cpp + src/call_back/livox_lidar_callback.cpp +) + +target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS}) + +# get include directories of custom msg headers if(HUMBLE_ROS STREQUAL "humble") + +rosidl_get_typesupport_target( + cpp_typesupport_target ${LIVOX_INTERFACES} "rosidl_typesupport_cpp" +) +target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}") + +# else() set(LIVOX_INTERFACE_TARGET +# "${LIVOX_INTERFACES}__rosidl_typesupport_cpp") +# add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES}) +# get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES +# ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES) endif() + +# include file direcotry +target_include_directories( + ${PROJECT_NAME} + PUBLIC ${LIVOX_LIDAR_SDK_INCLUDE_DIR} + ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs + src +) + +# link libraries +target_link_libraries( + ${PROJECT_NAME} ${LIVOX_LIDAR_SDK_LIBRARY} + ${LIVOX_INTERFACE_TARGET} # for custom msgs + ${PPT_LIBRARY} ${Boost_LIBRARY} +) + +rclcpp_components_register_node( + ${PROJECT_NAME} PLUGIN "livox_ros::DriverNode" EXECUTABLE + ${PROJECT_NAME}_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights uncomment + # the line when a copyright and license is not present in all source files + # set(ament_cmake_copyright_FOUND TRUE) the following line skips cpplint (only + # works in a git repo) uncomment the line when this package is not in a git + # repo set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE launch) 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/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/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..7fd8aeb86 --- /dev/null +++ b/packages/livox_driver/package.xml @@ -0,0 +1,30 @@ + + + + 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 + rcl_interfaces + + rosbag2 + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + + 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..e2189b851 --- /dev/null +++ b/packages/livox_driver/src/call_back/livox_lidar_callback.cpp @@ -0,0 +1,356 @@ +// +// 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; + // 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); + 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..23f476054 --- /dev/null +++ b/packages/livox_driver/src/driver_node.h @@ -0,0 +1,56 @@ +// +// 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; + +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_; +}; + +} // 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/ros_headers.h b/packages/livox_driver/src/include/ros_headers.h new file mode 100644 index 000000000..b43e52150 --- /dev/null +++ b/packages/livox_driver/src/include/ros_headers.h @@ -0,0 +1,51 @@ +// +// 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 "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 new file mode 100644 index 000000000..cd2ad4bc8 --- /dev/null +++ b/packages/livox_driver/src/lddc.cpp @@ -0,0 +1,404 @@ +// +// 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--------------------------------------------*/ +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; +} + +Lddc::~Lddc() { + PrepareExit(); + 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); + } + } +} + +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) { + 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); + } +} + +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; + } + cloud.header.stamp = rclcpp::Time(timestamp); + + 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) { + Publisher::SharedPtr publisher_ptr = + std::dynamic_pointer_cast >(GetCurrentPublisher(index)); + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(cloud); + } +} + +void Lddc::InitCustomMsg(CustomMsg& livox_msg, const StoragePacket& pkg, uint8_t index) { + livox_msg.header.frame_id.assign(frame_id_); + + uint64_t timestamp = 0; + if (!pkg.points.empty()) { + timestamp = pkg.base_time; + } + livox_msg.timebase = timestamp; + livox_msg.header.stamp = rclcpp::Time(timestamp); + + 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) { + Publisher::SharedPtr publisher_ptr = + std::dynamic_pointer_cast >(GetCurrentPublisher(index)); + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(livox_msg); + } +} + +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; + imu_msg.header.stamp = rclcpp::Time(timestamp); // to ros time stamp + + 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); + + Publisher::SharedPtr publisher_ptr = + std::dynamic_pointer_cast >(GetCurrentImuPublisher(index)); + + if (kOutputToRos == output_type_) { + publisher_ptr->publish(imu_msg); + } +} + +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); + } 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; + } +} + +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_; + } +} + +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 new file mode 100644 index 000000000..813d8f4ad --- /dev/null +++ b/packages/livox_driver/src/lddc.h @@ -0,0 +1,130 @@ +// +// 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 */ +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; + +class DriverNode; + +class Lddc final { + public: + Lddc( + int format, int multi_topic, int data_src, int output_type, double frq, + std::string& frame_id); + ~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 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 InitImuMsg(const ImuData& imu_data, ImuMsg& imu_msg, uint64_t& timestamp); + + void FillPointsToCustomMsg( + CustomMsg& livox_msg, LivoxPointXyzrtlt* src_point, uint32_t num, uint32_t offset_time, + uint32_t point_interval, uint32_t echo_num); + + PublisherPtr CreatePublisher(uint8_t msg_type, std::string& topic_name, uint32_t queue_size); + 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_; + + PublisherPtr private_pub_[kMaxSourceLidar]; + PublisherPtr global_pub_; + PublisherPtr private_imu_pub_[kMaxSourceLidar]; + PublisherPtr global_imu_pub_; + + 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..09281fd18 --- /dev/null +++ b/packages/livox_driver/src/lds_lidar.cpp @@ -0,0 +1,216 @@ +// +// 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() { + DisableLivoxSdkConsoleLogger(); + + // 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; + } + + // 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"); + } + + 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..e2cc6c601 --- /dev/null +++ b/packages/livox_driver/src/livox_driver.cpp @@ -0,0 +1,125 @@ +// +// 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; + +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->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; + } + + 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()); + + 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) + +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/packages/model/config/model.yaml b/packages/model/config/model.yaml index c60e9bc5d..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.0, y: 0.0, z: 0.0, w: 1.0 } + rotation: { x: 0.0, y: 0.0, z: 1.0, w: 0.0 } - frame_id: "base" child_frame_id: "camera_link" diff --git a/packages/truck/launch/lidar3d.yaml b/packages/truck/launch/lidar3d.yaml new file mode 100644 index 000000000..1ec564b09 --- /dev/null +++ b/packages/truck/launch/lidar3d.yaml @@ -0,0 +1,9 @@ +launch: +- include: + file: $(find-pkg-share livox_driver)/launch/livox_driver.py + arg: + - { 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" } 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 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