mirrors/livox_ros_driver
1
0
Fork 0
mirror of https://github.com/Livox-SDK/livox_ros_driver.git synced 2023-04-06 15:49:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

feature:add livox_ros_driver package

Browse Source
This commit is contained in:
Livox-SDK
2019-04-08 12:20:20 +08:00
commit 0c517daff0
17 changed files with 2841 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

262
livox_ros_driver/CMakeLists.txt Executable file
View File

@@ -0,0 +1,262 @@
cmake_minimum_required(VERSION 2.8.3)
project(livox_ros_driver)
set(PROJECT_LIDAR livox_lidar)
set(PROJECT_HUB livox_hub)
set(PROJECT_LIDAR_SRC ./livox_lidar.cpp)
set(PROJECT_HUB_SRC ./livox_hub.cpp)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
)
find_package(Boost REQUIRED)
find_package(Boost REQUIRED COMPONENTS thread)
## get pointcloud package
find_package(PCL REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
CustomPoint.msg
CustomMsg.msg
# Message2.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES lidar_sdk
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/lidar_sdk.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
##SET(SRC_DIR
## ./
##)
# traverse all source directories and add include dir
##FOREACH(dir ${SRC_DIR})
## MESSAGE(STATUS"src_dir: "${dir})
## include_directories(./include/${dir})
## include_directories(./${dir})
## AUX_SOURCE_DIRECTORY(${dir} source_list)
##ENDFOREACH()
find_package(PkgConfig)
pkg_check_modules(APR apr-1)
if (APR_FOUND)
message(${APR_INCLUDE_DIRS})
message(${APR_LIBRARIES})
endif (APR_FOUND)
include_directories(
./
${APR_INCLUDE_DIRS}
)
## PCL library
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(${PROJECT_LIDAR}_node
${PROJECT_LIDAR_SRC})
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_LIDAR}_node
livox_sdk_static.a
${APR_LIBRARIES}
${PCL_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
-lrt
)
## add hub project here
add_executable(${PROJECT_HUB}_node
${PROJECT_HUB_SRC})
target_link_libraries(${PROJECT_HUB}_node
livox_sdk_static.a
${APR_LIBRARIES}
${PCL_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
-lrt
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_lidar_sdk.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

189
livox_ros_driver/config/display_hub_points.rviz Normal file
View File

@@ -0,0 +1,189 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud1/Autocompute Value Bounds1
- /PointCloud21
Splitter Ratio: 0.500695
Tree Height: 747
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.856
Min Value: -0.735
Value: true
Axis: Z
Channel Name: x
Class: rviz/PointCloud
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: -0.088
Min Color: 0; 0; 0
Min Intensity: -1.951
Name: PointCloud
Position Transformer: XYZ
Queue Size: 1000
Selectable: true
Size (Pixels): 2
Size (m): 0.005
Style: Flat Squares
Topic: /livox/lidar
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.816
Min Value: -0.674
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1
Enabled: true
Invert Rainbow: true
Max Color: 255; 255; 255
Max Intensity: 151
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 1000
Selectable: true
Size (Pixels): 3
Size (m): 0.005
Style: Flat Squares
Topic: /livox/hub
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: livox_frame
Frame Rate: 20
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.61081
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.267255
Y: 0.0618536
Z: 0.150874
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.3048
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.01742
Saved:
- Class: rviz/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Name: Orbit
Near Clip Distance: 0.01
Pitch: 1.1104
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.570397
Window Geometry:
Displays:
collapsed: false
Height: 1028
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001830000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005110000003efc0100000002fb0000000800540069006d0065010000000000000511000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1297
X: 209
Y: 14

189
livox_ros_driver/config/display_lidar_points.rviz Normal file
View File

@@ -0,0 +1,189 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud1/Autocompute Value Bounds1
- /PointCloud21
Splitter Ratio: 0.500695
Tree Height: 747
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.856
Min Value: -0.735
Value: true
Axis: Z
Channel Name: x
Class: rviz/PointCloud
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: -0.088
Min Color: 0; 0; 0
Min Intensity: -1.951
Name: PointCloud
Position Transformer: XYZ
Queue Size: 1000
Selectable: true
Size (Pixels): 2
Size (m): 0.005
Style: Flat Squares
Topic: /livox/lidar
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.653
Min Value: -0.918
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 1
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 151
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 100
Selectable: true
Size (Pixels): 3
Size (m): 0.005
Style: Flat Squares
Topic: /livox/lidar
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: livox_frame
Frame Rate: 20
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 0.591995
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.267255
Y: 0.0618536
Z: 0.150874
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.229799
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.99561
Saved:
- Class: rviz/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Name: Orbit
Near Clip Distance: 0.01
Pitch: 1.1104
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.570397
Window Geometry:
Displays:
collapsed: false
Height: 1028
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001830000037afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005110000003efc0100000002fb0000000800540069006d0065010000000000000511000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003880000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1297
X: 407
Y: 14

14
livox_ros_driver/launch/livox_hub.launch Normal file
View File

@@ -0,0 +1,14 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver"
type="livox_hub_node" required="true"
output="screen" args="$(arg bd_list)"/>
<!--
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
-->
</launch>

14
livox_ros_driver/launch/livox_hub_msg.launch Normal file
View File

@@ -0,0 +1,14 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver"
type="livox_hub_node" required="true"
output="screen" args="$(arg bd_list)"/>
<!--
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
-->
</launch>

12
livox_ros_driver/launch/livox_hub_rviz.launch Normal file
View File

@@ -0,0 +1,12 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_hub_publisher" pkg="livox_ros_driver"
type="livox_hub_node" required="true"
output="screen" args="$(arg bd_list)"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_hub_points.rviz"/>
</launch>

13
livox_ros_driver/launch/livox_lidar.launch Normal file
View File

@@ -0,0 +1,13 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_lidar_node" required="true"
output="screen" args="$(arg bd_list)"/>
<!--
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
-->
</launch>

13
livox_ros_driver/launch/livox_lidar_msg.launch Normal file
View File

@@ -0,0 +1,13 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="1"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_lidar_node" required="true"
output="screen" args="$(arg bd_list)"/>
<!--
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
-->
</launch>

12
livox_ros_driver/launch/livox_lidar_rviz.launch Normal file
View File

@@ -0,0 +1,12 @@
<launch>
<arg name="bd_list" default="100000000000000"/>
<param name="livox_msg_type" value="0"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_lidar_node" required="true"
output="screen" args="$(arg bd_list)"/>
<node name="rviz" pkg="rviz" type="rviz" respawn="true"
args="-d $(find livox_ros_driver)/config/display_lidar_points.rviz"/>
</launch>

661
livox_ros_driver/livox_hub.cpp Executable file
View File

@@ -0,0 +1,661 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 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 <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#include <time.h>
#include <vector>
#include <chrono>
#include "livox_sdk.h"
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h>
#define kMaxPointPerEthPacket (100)
#define kMaxStoragePackets (128) // must be 2^n
#define KEthPacketHeaderLength (18) //(sizeof(LivoxEthPacket) - 1)
#define KEthPacketMaxLength (1500)
#define KCartesianPointSize (13)
#define KSphericalPointSzie (9)
const uint64_t kPacketTimeGap = 1000000; // 1ms = 1000000ns
const uint64_t kMaxPacketTimeGap = 1700000; // the threshold of packet continuous
const uint64_t kNsPerSecond = 1000000000; // 1s = 1000000000ns
const uint32_t kPublishIntervalMs = 50; // unit:ms
#define BD_ARGC_NUM (4)
#define BD_ARGV_POS (1)
#define COMMANDLINE_BD_SIZE (15)
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
#pragma pack(1)
typedef struct {
uint64_t time_rcv; // used for pps sync only mode
uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket;
typedef struct {
uint8_t lidar_id;
uint8_t rsvd[3];
uint32_t point_num;
uint64_t timestamp; // ns
LivoxPoint *point;
} PublishPacket;
#pragma pack()
typedef struct {
StoragePacket *storage_packet[kMaxStoragePackets]; // size must be 2^n
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; // must be 2^n
} StoragePacketQueue;
typedef struct {
uint32_t receive_packet_count;
uint32_t loss_packet_count;
uint64_t last_timestamp;
uint64_t timebase; // unit:nanosecond
uint32_t timebase_state;
} LidarPacketStatistic;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum {
kPointCloud2Msg = 0,
kLivoxCustomMsg
} LivoxMsgType;
StoragePacketQueue storage_packet_pool[kMaxLidarCount];
/* for global publisher use */
ros::Publisher cloud_pub;
/* for device connect use ----------------------------------------------------------------------- */
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
kDeviceStateSampling = 2,
} DeviceState;
typedef struct {
uint8_t handle;
DeviceState device_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
} DeviceItem;
DeviceItem lidars[kMaxLidarCount];
DeviceItem hub;
/* user add hub broadcast code here */
const char* broadcast_code_list[] = {
"000000000000001",
};
#define BROADCAST_CODE_LIST_SIZE (sizeof(broadcast_code_list) / sizeof(intptr_t))
/* total broadcast code, include broadcast_code_list and commandline input */
std::vector<std::string > total_broadcast_code;
/* for pointcloud queue process */
void InitQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = kMaxStoragePackets;
queue->mask = kMaxStoragePackets - 1;
for (int i=0; i<kMaxStoragePackets; i++) {
queue->storage_packet[i] = new StoragePacket;
}
}
void ResetQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
void InitStoragePacketPool(void) {
for (int i=0; i<kMaxLidarCount; i++) {
InitQueue(&storage_packet_pool[i]);
}
}
uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
uint32_t mask = queue->mask;
uint32_t rd_idx = queue->rd_idx & mask;
memcpy(storage_packet, queue->storage_packet[rd_idx], sizeof(StoragePacket));
queue->rd_idx++;
return 1;
}
uint32_t QueueUsedSize(StoragePacketQueue *queue) {
return (queue->wr_idx - queue->rd_idx) & queue->mask;
}
uint32_t QueueIsFull(StoragePacketQueue *queue) {
return ((queue->wr_idx + 1) == queue->rd_idx);
}
uint32_t QueueIsEmpty(StoragePacketQueue *queue) {
return (queue->rd_idx == queue->wr_idx);
}
static uint32_t GetEthPacketLen(LivoxEthPacket* eth_packet, uint32_t point_num) {
if (kCoordinateCartesian == eth_packet->data_type) {
return (KEthPacketHeaderLength + point_num * KCartesianPointSize);
} else {
return (KEthPacketHeaderLength + point_num * KSphericalPointSzie);
}
}
uint32_t PushEthPacketToStorageQueue(StoragePacketQueue* queue, LivoxEthPacket* eth_packet, \
uint32_t point_num, uint64_t timebase) {
uint32_t mask = queue->mask;
uint32_t wr_idx = queue->wr_idx & mask;
queue->storage_packet[wr_idx]->time_rcv = timebase;
queue->storage_packet[wr_idx]->point_num = point_num;
memcpy(queue->storage_packet[wr_idx]->raw_data, \
reinterpret_cast<char *>(eth_packet), \
GetEthPacketLen(eth_packet, point_num));
queue->wr_idx++;
return 1;
}
static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
uint64_t timestamp = *((uint64_t *)(raw_packet->timestamp));
if (raw_packet->timestamp_type == kTimestampTypePps) {
return (timestamp + packet->time_rcv);
} else if (raw_packet->timestamp_type == kTimestampTypeNoSync) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePtp) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
//ROS_INFO("UTC:%d %d %d %d", time_utc.tm_year, time_utc.tm_mon,\
// time_utc.tm_mday, time_utc.tm_hour);
uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + *((uint32_t *)(&(raw_packet->timestamp[4]))); // to us
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
ROS_INFO("timestamp type invalid");
return 0;
}
}
/* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud);
cloud->header.frame_id = "livox_frame";
cloud->height = 1;
cloud->width = 0;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueuePop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!cloud->width) {
cloud->header.stamp = timestamp / 1000; // to us
ROS_DEBUG("[%d]:%ld us", handle, cloud->header.stamp);
}
cloud->width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.intensity = (float) raw_points->reflectivity;
++raw_points;
cloud->points.push_back(point);
}
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", cloud->width);
cloud_pub.publish(cloud);
return published_packet;
}
/* for pointcloud convert process */
static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t packet_num,\
uint8_t handle) {
static uint32_t msg_seq = 0;
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init livox custom msg */
livox_ros_driver::CustomMsg livox_msg;
livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq;
++msg_seq;
livox_msg.header.stamp = ros::Time::now();
livox_msg.timebase = 0;
livox_msg.timestep = 10000; // 10us = 10^4ns;
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueuePop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!livox_msg.timebase) {
livox_msg.timebase = timestamp; // to us
ROS_DEBUG("[%d]:%ld", handle, livox_msg.timebase);
}
livox_msg.point_num += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.reflectivity = raw_points->reflectivity;
point.line = 0;
++raw_points;
livox_msg.points.push_back(point);
}
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", livox_msg.point_num);
cloud_pub.publish(livox_msg);
return published_packet;
}
static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point) {
p_dpoint->x = p_raw_point->x/1000.0f;
p_dpoint->y = p_raw_point->y/1000.0f;
p_dpoint->z = p_raw_point->z/1000.0f;
p_dpoint->reflectivity = p_raw_point->reflectivity;
}
void GetLidarData(uint8_t hub_handle, LivoxEthPacket *data, uint32_t data_num) {
using namespace std;
LivoxEthPacket *lidar_pack = data;
if (!data || !data_num) {
return;
}
/* caculate which lidar this eth packet data belong to */
uint8_t handle = HubGetLidarHandle(lidar_pack->slot, lidar_pack->id);
if (handle >= kMaxLidarCount) {
return;
}
LidarPacketStatistic *packet_statistic = &lidars[handle].statistic_info;
uint64_t cur_timestamp = *((uint64_t *)(lidar_pack->timestamp));
if (lidar_pack->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp < packet_statistic->last_timestamp) && \
(cur_timestamp < kPacketTimeGap)) { // sync point
auto cur_time = chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time;
//ROS_DEBUG("sysnc time : %lu %lu %lu", packet_statistic->timebase, cur_timestamp, \
// packet_statistic->last_timestamp);
}
}
packet_statistic->last_timestamp = cur_timestamp;
StoragePacketQueue *p_queue = &storage_packet_pool[handle];
if (!QueueIsFull(p_queue)) {
if (data_num <= kMaxPointPerEthPacket) {
PushEthPacketToStorageQueue(p_queue, lidar_pack, data_num, packet_statistic->timebase);
}
}
}
void PollPointcloudData(int msg_type) {
for (int i = 0; i < kMaxLidarCount; i++) {
StoragePacketQueue *p_queue = &storage_packet_pool[i];
if (kDeviceStateSampling != lidars[i].device_state) {
continue;
}
while (!QueueIsEmpty(p_queue)) {
//ROS_DEBUG("%d %d %d %d\r\n", i, p_queue->rd_idx, p_queue->wr_idx, QueueUsedSize(p_queue));
uint32_t used_size = QueueUsedSize(p_queue);
if (kPointCloud2Msg == msg_type) {
if (used_size == PublishPointcloud2(p_queue, used_size, i)) {
break;
}
} else {
if (used_size == PublishCustomPointcloud(p_queue, QueueUsedSize(p_queue), i)) {
break;
}
}
}
}
}
/** add bd to total_broadcast_code */
void AddBroadcastCode(const char* bd_str) {
total_broadcast_code.push_back(bd_str);
}
/** add bd in broadcast_code_list to total_broadcast_code */
void AddLocalBroadcastCode(void) {
for (int i = 0; i < BROADCAST_CODE_LIST_SIZE; ++i) {
std::string invalid_bd = "000000000";
ROS_INFO("broadcast code list :%s", broadcast_code_list[i]);
if ((COMMANDLINE_BD_SIZE == strlen(broadcast_code_list[i])) && \
(NULL == strstr(broadcast_code_list[i], invalid_bd.c_str()))) {
AddBroadcastCode(broadcast_code_list[i]);
} else {
ROS_INFO("Invalid bd:%s", broadcast_code_list[i]);
}
}
}
/** add commandline bd to total_broadcast_code */
void AddCommandlineBroadcastCode(const char* cammandline_str) {
char* strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str);
std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str());
std::string invalid_bd = "000000000";
while (bd_str != NULL) {
ROS_INFO("commandline input bd:%s", bd_str);
if ((COMMANDLINE_BD_SIZE == strlen(bd_str)) && \
(NULL == strstr(bd_str, invalid_bd.c_str()))) {
AddBroadcastCode(bd_str);
} else {
ROS_INFO("Invalid bd:%s", bd_str);
}
bd_str = strtok(NULL, pattern.c_str());
}
delete [] strs;
}
/* control hub ---------------------------------------------------------------------------------- */
void OnSampleCallback(uint8_t status, uint8_t hub_handle, uint8_t response, void *data) {
ROS_INFO("OnSampleCallback statue %d handle %d response %d \n", status, hub_handle, response);
if (status == kStatusSuccess) {
if (response != 0) {
hub.device_state = kDeviceStateConnect;
}
} else if (status == kStatusTimeout) {
hub.device_state = kDeviceStateConnect;
}
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t hub_handle, uint8_t response, void *data) {
}
/** Query the firmware version of Livox Hub. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
ROS_INFO("Device Query Informations Failed %d", status);
}
if (ack) {
ROS_INFO("firm ver: %d.%d.%d.%d",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]);
}
}
void OnHubLidarInfo(uint8_t status, uint8_t handle, HubQueryLidarInformationResponse *response, void *client_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed %d\n", status);
}
if (response) {
int i = 0;
for (i = 0; i < response->count; ++i) {
printf("Hub Lidar Info broadcast code %s id %d slot %d \n ",
response->device_info_list[i].broadcast_code,
response->device_info_list[i].id,
response->device_info_list[i].slot);
}
}
}
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
ROS_INFO("OnDeviceChange broadcast code %s update type %d", info->broadcast_code, type);
uint8_t hub_handle = info->handle;
if (hub_handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
DeviceInfo *_lidars = (DeviceInfo *) malloc(sizeof(DeviceInfo) * kMaxLidarCount);
uint8_t count = kMaxLidarCount;
uint8_t status = GetConnectedDevices(_lidars, &count);
if (status == kStatusSuccess) {
int i = 0;
for (i = 0; i < count; ++i) {
uint8_t handle = _lidars[i].handle;
if (handle < kMaxLidarCount) {
lidars[handle].handle = handle;
lidars[handle].info = _lidars[i];
lidars[handle].device_state = kDeviceStateSampling;
ROS_INFO("lidar %d : %s\r\n", _lidars[i].handle, _lidars[i].broadcast_code);
}
}
}
if (_lidars) {
free(_lidars);
}
if (info->type == kDeviceTypeHub) {
HubQueryLidarInformation(OnHubLidarInfo, NULL);
}
if (hub.device_state == kDeviceStateDisconnect) {
hub.device_state = kDeviceStateConnect;
hub.info = *info;
}
} else if (type == kEventDisconnect) {
hub.device_state = kDeviceStateDisconnect;
} else if (type == kEventStateChange) {
hub.info = *info;
}
if (hub.device_state == kDeviceStateConnect) {
ROS_INFO("Device State status_code %d", hub.info.status.status_code);
ROS_INFO("Device State working state %d", hub.info.state);
ROS_INFO("Device feature %d", hub.info.feature);
if (hub.info.state == kLidarStateNormal) {
HubStartSampling(OnSampleCallback, NULL);
hub.device_state = kDeviceStateSampling;
}
}
}
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
return;
}
ROS_INFO("Receive Broadcast Code %s, please add it to broacast_code_list if want to connect!\n",\
info->broadcast_code);
if (total_broadcast_code.size() > 0) {
bool found = false;
for (int i = 0; i < total_broadcast_code.size(); ++i) {
if (strncmp(info->broadcast_code, total_broadcast_code[i].c_str(), kBroadcastCodeSize) == 0) {
found = true;
break;
}
}
if (!found) {
ROS_INFO("Not in the broacast_code_list, please add it to if want to connect!");
return;
}
} else {
ROS_INFO("In automatic connection mode, will connect %s", info->broadcast_code);
}
bool result = false;
uint8_t hub_handle = 0;
result = AddHubToConnect(info->broadcast_code, &hub_handle);
if (result == kStatusSuccess && hub_handle < kMaxLidarCount) {
ROS_INFO("set callback");
SetDataCallback(hub_handle, GetLidarData);
hub.handle = hub_handle;
hub.device_state = kDeviceStateDisconnect;
}
}
int main(int argc, char **argv) {
if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) {
ros::console::notifyLoggerLevelsChanged();
}
ROS_INFO("Livox-SDK ros demo");
InitStoragePacketPool();
if (!Init()) {
ROS_FATAL("Livox-SDK init fail!");
return -1;
}
AddLocalBroadcastCode();
if (argc >= BD_ARGC_NUM) {
ROS_INFO("Commandline input %s", argv[BD_ARGV_POS]);
AddCommandlineBroadcastCode(argv[BD_ARGV_POS]);
}
if (total_broadcast_code.size() > 0) {
ROS_INFO("list all valid bd:");
for (int i = 0; i < total_broadcast_code.size(); ++i) {
ROS_INFO("%s", total_broadcast_code[i].c_str());
}
} else {
ROS_INFO("No valid bd input, switch to automatic connection mode!");
}
memset(lidars, 0, sizeof(lidars));
memset(&hub, 0, sizeof(hub));
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
if (!Start()) {
Uninit();
return -1;
}
/* ros related */
ros::init(argc, argv, "livox_hub_publisher");
ros::NodeHandle livox_node;
int msg_type;
livox_node.getParam("livox_msg_type", msg_type);
if (kPointCloud2Msg == msg_type) {
cloud_pub = livox_node.advertise<PointCloud>("livox/hub", kMaxStoragePackets);
ROS_INFO("Publish PointCloud2");
} else {
cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/hub", \
kMaxStoragePackets);
ROS_INFO("Publish Livox Custom Msg");
}
ros::Time::init();
ros::Rate r(1000.0 / kPublishIntervalMs); // 1000.0 / x = hz
while (ros::ok()) {
PollPointcloudData(msg_type);
r.sleep();
}
Uninit();
}

616
livox_ros_driver/livox_lidar.cpp Executable file
View File

@@ -0,0 +1,616 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 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 <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <math.h>
#include <time.h>
#include <vector>
#include <chrono>
#include "livox_sdk.h"
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <livox_ros_driver/CustomPoint.h>
#include <livox_ros_driver/CustomMsg.h>
#define kMaxPointPerEthPacket (100)
#define kMaxStoragePackets (128) // must be 2^n
#define KEthPacketHeaderLength (18) //(sizeof(LivoxEthPacket) - 1)
#define KEthPacketMaxLength (1500)
#define KCartesianPointSize (13)
#define KSphericalPointSzie (9)
const uint64_t kPacketTimeGap = 1000000; // 1ms = 1000000ns
const uint64_t kMaxPacketTimeGap = 1700000; // the threshold of packet continuous
const uint64_t kNsPerSecond = 1000000000; // 1s = 1000000000ns
const uint32_t kPublishIntervalMs = 50; // unit:ms
#define BD_ARGC_NUM (4)
#define BD_ARGV_POS (1)
#define COMMANDLINE_BD_SIZE (15)
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
#pragma pack(1)
typedef struct {
uint64_t time_rcv; // used for pps sync only mode
uint32_t point_num;
uint8_t raw_data[KEthPacketMaxLength];
} StoragePacket;
typedef struct {
uint8_t lidar_id;
uint8_t rsvd[3];
uint32_t point_num;
uint64_t timestamp; // ns
LivoxPoint *point;
} PublishPacket;
#pragma pack()
typedef struct {
StoragePacket *storage_packet[kMaxStoragePackets]; // size must be 2^n
volatile uint32_t rd_idx;
volatile uint32_t wr_idx;
uint32_t mask;
uint32_t size; // must be 2^n
} StoragePacketQueue;
typedef struct {
uint32_t receive_packet_count;
uint32_t loss_packet_count;
uint64_t last_timestamp;
uint64_t timebase; // unit:nanosecond
uint32_t timebase_state;
} LidarPacketStatistic;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef enum {
kPointCloud2Msg = 0,
kLivoxCustomMsg
} LivoxMsgType;
StoragePacketQueue storage_packet_pool[kMaxLidarCount];
/* for global publisher use */
ros::Publisher cloud_pub;
/* for device connect use ----------------------------------------------------------------------- */
typedef enum {
kDeviceStateDisconnect = 0,
kDeviceStateConnect = 1,
kDeviceStateSampling = 2,
} DeviceState;
typedef struct {
uint8_t handle;
DeviceState device_state;
DeviceInfo info;
LidarPacketStatistic statistic_info;
} DeviceItem;
DeviceItem lidars[kMaxLidarCount];
/* user add broadcast code here */
const char* broadcast_code_list[] = {
"000000000000001",
};
#define BROADCAST_CODE_LIST_SIZE (sizeof(broadcast_code_list) / sizeof(intptr_t))
/* total broadcast code, include broadcast_code_list and commandline input */
std::vector<std::string > total_broadcast_code;
/* for pointcloud queue process */
void InitQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
queue->size = kMaxStoragePackets;
queue->mask = kMaxStoragePackets - 1;
for (int i=0; i<kMaxStoragePackets; i++) {
queue->storage_packet[i] = new StoragePacket;
}
}
void ResetQueue(StoragePacketQueue* queue) {
queue->rd_idx = 0;
queue->wr_idx = 0;
}
void InitStoragePacketPool(void) {
for (int i=0; i<kMaxLidarCount; i++) {
InitQueue(&storage_packet_pool[i]);
}
}
uint32_t QueuePop(StoragePacketQueue* queue, StoragePacket* storage_packet) {
uint32_t mask = queue->mask;
uint32_t rd_idx = queue->rd_idx & mask;
memcpy(storage_packet, queue->storage_packet[rd_idx], sizeof(StoragePacket));
queue->rd_idx++;
return 1;
}
uint32_t QueueUsedSize(StoragePacketQueue *queue) {
return (queue->wr_idx - queue->rd_idx) & queue->mask;
}
uint32_t QueueIsFull(StoragePacketQueue *queue) {
return ((queue->wr_idx + 1) == queue->rd_idx);
}
uint32_t QueueIsEmpty(StoragePacketQueue *queue) {
return (queue->rd_idx == queue->wr_idx);
}
static uint32_t GetEthPacketLen(LivoxEthPacket* eth_packet, uint32_t point_num) {
if (kCoordinateCartesian == eth_packet->data_type) {
return (KEthPacketHeaderLength + point_num * KCartesianPointSize);
} else {
return (KEthPacketHeaderLength + point_num * KSphericalPointSzie);
}
}
uint32_t PushEthPacketToStorageQueue(StoragePacketQueue* queue, LivoxEthPacket* eth_packet, \
uint32_t point_num, uint64_t timebase) {
uint32_t mask = queue->mask;
uint32_t wr_idx = queue->wr_idx & mask;
queue->storage_packet[wr_idx]->time_rcv = timebase;
queue->storage_packet[wr_idx]->point_num = point_num;
memcpy(queue->storage_packet[wr_idx]->raw_data, \
reinterpret_cast<char *>(eth_packet), \
GetEthPacketLen(eth_packet, point_num));
queue->wr_idx++;
return 1;
}
static uint64_t GetStoragePacketTimestamp(StoragePacket* packet) {
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(packet->raw_data);
uint64_t timestamp = *((uint64_t *)(raw_packet->timestamp));
if (raw_packet->timestamp_type == kTimestampTypePps) {
return (timestamp + packet->time_rcv);
} else if (raw_packet->timestamp_type == kTimestampTypeNoSync) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePtp) {
return timestamp;
} else if (raw_packet->timestamp_type == kTimestampTypePpsGps) {
struct tm time_utc;
time_utc.tm_isdst = 0;
time_utc.tm_year = raw_packet->timestamp[0] + 100; // map 2000 to 1990
time_utc.tm_mon = raw_packet->timestamp[1];
time_utc.tm_mday = raw_packet->timestamp[2];
time_utc.tm_hour = raw_packet->timestamp[3];
time_utc.tm_min = 0;
time_utc.tm_sec = 0;
uint64_t time_epoch = mktime(&time_utc);
time_epoch = time_epoch * 1000000 + *((uint32_t *)(&(raw_packet->timestamp[4]))); // to us
time_epoch = time_epoch * 1000; // to ns
return time_epoch;
} else {
ROS_INFO("timestamp type invalid");
return 0;
}
}
/* for pointcloud convert process */
static uint32_t PublishPointcloud2(StoragePacketQueue* queue, uint32_t packet_num, \
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init point cloud data struct */
PointCloud::Ptr cloud (new PointCloud);
cloud->header.frame_id = "livox_frame";
cloud->height = 1;
cloud->width = 0;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueuePop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!cloud->width) {
cloud->header.stamp = timestamp / 1000; // to us
ROS_DEBUG("[%d]:%ld us", handle, cloud->header.stamp);
}
cloud->width += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
pcl::PointXYZI point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.intensity = (float) raw_points->reflectivity;
++raw_points;
cloud->points.push_back(point);
}
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", cloud->width);
cloud_pub.publish(cloud);
return published_packet;
}
/* for pointcloud convert process */
static uint32_t PublishCustomPointcloud(StoragePacketQueue* queue, uint32_t packet_num,\
uint8_t handle) {
static uint32_t msg_seq = 0;
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
/* init livox custom msg */
livox_ros_driver::CustomMsg livox_msg;
livox_msg.header.frame_id = "livox_frame";
livox_msg.header.seq = msg_seq;
++msg_seq;
livox_msg.header.stamp = ros::Time::now();
livox_msg.timebase = 0;
livox_msg.timestep = 10000; // 10us = 10^4ns;
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueuePop(queue, &storage_packet);
LivoxEthPacket* raw_packet = reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
LivoxRawPoint* raw_points = reinterpret_cast<LivoxRawPoint *>(raw_packet->data);
timestamp = GetStoragePacketTimestamp(&storage_packet);
if (published_packet && \
((timestamp - last_timestamp) > kMaxPacketTimeGap)) {
ROS_INFO("packet loss : %ld", timestamp);
break;
}
if (!livox_msg.timebase) {
livox_msg.timebase = timestamp; // to us
ROS_DEBUG("[%d]:%ld", handle, livox_msg.timebase);
}
livox_msg.point_num += storage_packet.point_num;
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
livox_ros_driver::CustomPoint point;
point.x = raw_points->x/1000.0f;
point.y = raw_points->y/1000.0f;
point.z = raw_points->z/1000.0f;
point.reflectivity = raw_points->reflectivity;
point.line = 0;
++raw_points;
livox_msg.points.push_back(point);
}
last_timestamp = timestamp;
++published_packet;
}
//ROS_INFO("%d", livox_msg.point_num);
cloud_pub.publish(livox_msg);
return published_packet;
}
static void PointCloudConvert(LivoxPoint *p_dpoint, LivoxRawPoint *p_raw_point) {
p_dpoint->x = p_raw_point->x/1000.0f;
p_dpoint->y = p_raw_point->y/1000.0f;
p_dpoint->z = p_raw_point->z/1000.0f;
p_dpoint->reflectivity = p_raw_point->reflectivity;
}
void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num) {
using namespace std;
LivoxEthPacket* lidar_pack = data;
if (!data || !data_num || (handle >= kMaxLidarCount)) {
return;
}
LidarPacketStatistic *packet_statistic = &lidars[handle].statistic_info;
uint64_t cur_timestamp = *((uint64_t *)(lidar_pack->timestamp));
if (lidar_pack->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp < packet_statistic->last_timestamp) && \
(cur_timestamp < kPacketTimeGap)) { // sync point
auto cur_time = chrono::high_resolution_clock::now();
int64_t sync_time = cur_time.time_since_epoch().count();
packet_statistic->timebase = sync_time;
//ROS_DEBUG("sysnc time : %lu %lu %lu", packet_statistic->timebase, cur_timestamp, \
// packet_statistic->last_timestamp);
}
}
packet_statistic->last_timestamp = cur_timestamp;
StoragePacketQueue *p_queue = &storage_packet_pool[handle];
if (!QueueIsFull(p_queue)) {
if (data_num <= kMaxPointPerEthPacket) {
PushEthPacketToStorageQueue(p_queue, lidar_pack, data_num, packet_statistic->timebase);
}
}
}
void PollPointcloudData(int msg_type) {
for (int i = 0; i < kMaxLidarCount; i++) {
StoragePacketQueue *p_queue = &storage_packet_pool[i];
if (kDeviceStateSampling != lidars[i].device_state) {
continue;
}
while (!QueueIsEmpty(p_queue)) {
//ROS_DEBUG("%d %d %d %d\r\n", i, p_queue->rd_idx, p_queue->wr_idx, QueueUsedSize(p_queue));
uint32_t used_size = QueueUsedSize(p_queue);
if (kPointCloud2Msg == msg_type) {
if (used_size == PublishPointcloud2(p_queue, used_size, i)) {
break;
}
} else {
if (used_size == PublishCustomPointcloud(p_queue, QueueUsedSize(p_queue), i)) {
break;
}
}
}
}
}
/** add bd to total_broadcast_code */
void AddBroadcastCode(const char* bd_str) {
total_broadcast_code.push_back(bd_str);
}
/** add bd in broadcast_code_list to total_broadcast_code */
void AddLocalBroadcastCode(void) {
for (int i = 0; i < BROADCAST_CODE_LIST_SIZE; ++i) {
std::string invalid_bd = "000000000";
ROS_INFO("broadcast code list :%s", broadcast_code_list[i]);
if ((COMMANDLINE_BD_SIZE == strlen(broadcast_code_list[i])) && \
(NULL == strstr(broadcast_code_list[i], invalid_bd.c_str()))) {
AddBroadcastCode(broadcast_code_list[i]);
} else {
ROS_INFO("Invalid bd:%s", broadcast_code_list[i]);
}
}
}
/** add commandline bd to total_broadcast_code */
void AddCommandlineBroadcastCode(const char* cammandline_str) {
char* strs = new char[strlen(cammandline_str) + 1];
strcpy(strs, cammandline_str);
std::string pattern = "&";
char* bd_str = strtok(strs, pattern.c_str());
std::string invalid_bd = "000000000";
while (bd_str != NULL) {
ROS_INFO("commandline input bd:%s", bd_str);
if ((COMMANDLINE_BD_SIZE == strlen(bd_str)) && \
(NULL == strstr(bd_str, invalid_bd.c_str()))) {
AddBroadcastCode(bd_str);
} else {
ROS_INFO("Invalid bd:%s", bd_str);
}
bd_str = strtok(NULL, pattern.c_str());
}
delete [] strs;
}
/** Callback function of starting sampling. */
void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
ROS_INFO("OnSampleCallback statue %d handle %d response %d", status, handle, response);
if (status == kStatusSuccess) {
if (response != 0) {
lidars[handle].device_state = kDeviceStateConnect;
}
} else if (status == kStatusTimeout) {
lidars[handle].device_state = kDeviceStateConnect;
}
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
}
/** Query the firmware version of Livox LiDAR. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
ROS_INFO("Device Query Informations Failed %d", status);
}
if (ack) {
ROS_INFO("firm ver: %d.%d.%d.%d",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]);
}
}
/** Callback function of changing of device state. */
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
ROS_INFO("OnDeviceChange broadcast code %s update type %d", info->broadcast_code, type);
uint8_t handle = info->handle;
if (handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
QueryDeviceInformation(handle, OnDeviceInformation, NULL);
if (lidars[handle].device_state == kDeviceStateDisconnect) {
lidars[handle].device_state = kDeviceStateConnect;
lidars[handle].info = *info;
}
} else if (type == kEventDisconnect) {
lidars[handle].device_state = kDeviceStateDisconnect;
} else if (type == kEventStateChange) {
lidars[handle].info = *info;
}
if (lidars[handle].device_state == kDeviceStateConnect) {
ROS_INFO("Device State status_code %d", lidars[handle].info.status.status_code);
ROS_INFO("Device State working state %d", lidars[handle].info.state);
ROS_INFO("Device feature %d", lidars[handle].info.feature);
if (lidars[handle].info.state == kLidarStateNormal) {
if (lidars[handle].info.type == kDeviceTypeHub) {
HubStartSampling(OnSampleCallback, NULL);
} else {
LidarStartSampling(handle, OnSampleCallback, NULL);
}
lidars[handle].device_state = kDeviceStateSampling;
}
}
}
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
return;
}
ROS_INFO("Receive Broadcast Code %s, please add it to broacast_code_list if want to connect!\n",\
info->broadcast_code);
if (total_broadcast_code.size() > 0) {
bool found = false;
for (int i = 0; i < total_broadcast_code.size(); ++i) {
if (strncmp(info->broadcast_code, total_broadcast_code[i].c_str(), kBroadcastCodeSize) == 0) {
found = true;
break;
}
}
if (!found) {
ROS_INFO("Not in the broacast_code_list, please add it to if want to connect!");
return;
}
} else {
ROS_INFO("In automatic connection mode, will connect %s", info->broadcast_code);
}
bool result = false;
uint8_t handle = 0;
result = AddLidarToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, GetLidarData);
lidars[handle].handle = handle;
lidars[handle].device_state = kDeviceStateDisconnect;
}
}
int main(int argc, char **argv) {
if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info)) {
ros::console::notifyLoggerLevelsChanged();
}
ROS_INFO("Livox-SDK ros demo");
InitStoragePacketPool();
if (!Init()) {
ROS_FATAL("Livox-SDK init fail!");
return -1;
}
AddLocalBroadcastCode();
if (argc >= BD_ARGC_NUM) {
ROS_INFO("Commandline input %s", argv[BD_ARGV_POS]);
AddCommandlineBroadcastCode(argv[BD_ARGV_POS]);
}
if (total_broadcast_code.size() > 0) {
ROS_INFO("list all valid bd:");
for (int i = 0; i < total_broadcast_code.size(); ++i) {
ROS_INFO("%s", total_broadcast_code[i].c_str());
}
} else {
ROS_INFO("No valid bd input, switch to automatic connection mode!");
}
memset(lidars, 0, sizeof(lidars));
SetBroadcastCallback(OnDeviceBroadcast);
SetDeviceStateUpdateCallback(OnDeviceChange);
if (!Start()) {
Uninit();
return -1;
}
/* ros related */
ros::init(argc, argv, "livox_lidar_publisher");
ros::NodeHandle livox_node;
int msg_type;
livox_node.getParam("livox_msg_type", msg_type);
if (kPointCloud2Msg == msg_type) {
cloud_pub = livox_node.advertise<PointCloud>("livox/lidar", kMaxStoragePackets);
ROS_INFO("Publish PointCloud2");
} else {
cloud_pub = livox_node.advertise<livox_ros_driver::CustomMsg>("livox/lidar", \
kMaxStoragePackets);
ROS_INFO("Publish Livox Custom Msg");
}
ros::Time::init();
ros::Rate r(1000.0 / kPublishIntervalMs); // 1000.0 / x = hz
while (ros::ok()) {
PollPointcloudData(msg_type);
r.sleep();
}
Uninit();
}

10
livox_ros_driver/msg/CustomMsg.msg Normal file
View File

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

8
livox_ros_driver/msg/CustomPoint.msg Normal file
View File

@@ -0,0 +1,8 @@
# Livox costom pointcloud format.
float32 x # X axis, unit:m
float32 y # Y axis, unit:m
float32 z # Z axis, unit:m
uint8 reflectivity # reflectivity, 0~255
uint8 line # laser number in lidar

72
livox_ros_driver/package.xml Normal file
View File

@@ -0,0 +1,72 @@
<?xml version="1.0"?>
<package format="2">
<name>livox_ros_driver</name>
<version>1.1.0</version>
<description>The livox ros driver package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dev@livoxtech.com">xxx</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>MIT</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/lidar_sdk</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>