diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..bbcc46d6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.vscode +build +dedicated_sdk/build +/CMakeList.txt +package.xml \ No newline at end of file diff --git a/CMakeLists_ROS1.txt b/CMakeLists_ROS1.txt index a314a941..c5d61e1a 100644 --- a/CMakeLists_ROS1.txt +++ b/CMakeLists_ROS1.txt @@ -90,7 +90,6 @@ set(CMAKE_CXX_EXTENSIONS OFF) ## make sure the livox_sdk_static library is installed find_library(LIVOX_SDK_LIBRARY liblivox_sdk_static.a /usr/local/lib) find_library(LIVOX_SDK_VEHICLE_LIBRARY liblivox_sdk_vehicle_static.a /usr/local/lib) -find_library(LIVOX_SDK_DIRECT_LIBRARY liblivox_sdk_direct_static.a /usr/local/lib) find_library(LIVOX_SDK_COMMON_LIBRARY liblivox_sdk_common_static.a /usr/local/lib) find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_static.a /usr/local/lib) @@ -98,7 +97,6 @@ include_directories( ./ ${CMAKE_CURRENT_SOURCE_DIR}/livox_sdk/sdk_core/include ${CMAKE_CURRENT_SOURCE_DIR}/livox_sdk_vehicle/sdk_core/include - ${CMAKE_CURRENT_SOURCE_DIR}/livox_sdk_direct/sdk_core/include ${CMAKE_CURRENT_SOURCE_DIR}/livox_sdk_common/include ) @@ -106,7 +104,6 @@ link_directories( ./ ${CMAKE_CURRENT_SOURCE_DIR}/livox_sdk/build/sdk_core ${CMAKE_CURRENT_SOURCE_DIR}/livox_sdk_vehicle/build/sdk_core - ${CMAKE_CURRENT_SOURCE_DIR}/livox_sdk_direct/build/sdk_core ) @@ -144,7 +141,7 @@ target_sources(${PROJECT_NAME}_node src/lds_lidar.cpp src/lds_hub.cpp src/lddc.cpp - src/livox_ros_driver.cpp + src/livox_ros_driver2.cpp src/vehicle_lidar_thread.cpp src/comm/comm.cpp @@ -154,14 +151,12 @@ target_sources(${PROJECT_NAME}_node src/comm/cache_index.cpp src/parse_cfg_file/parse_cfg_file.cpp - src/parse_cfg_file/parse_direct_lidar_cfg.cpp src/parse_cfg_file/parse_industrial_lidar_cfg.cpp src/parse_cfg_file/parse_vehicle_lidar_cfg.cpp src/parse_cfg_file/parse_livox_lidar_cfg.cpp src/call_back/industrial_lidar_callback.cpp src/call_back/vehicle_lidar_callback.cpp - src/call_back/direct_lidar_callback.cpp src/call_back/lidar_common_callback.cpp src/call_back/hub_callback.cpp src/call_back/livox_lidar_callback.cpp @@ -196,7 +191,6 @@ target_include_directories(${PROJECT_NAME}_node target_link_libraries(${PROJECT_NAME}_node ${LIVOX_SDK_LIBRARY} ${LIVOX_SDK_VEHICLE_LIBRARY} - ${LIVOX_SDK_DIRECT_LIBRARY} ${LIVOX_SDK_COMMON_LIBRARY} ${LIVOX_LIDAR_SDK_LIBRARY} ${Boost_LIBRARY} diff --git a/README.md b/README.md index 19218c8a..98d069bd 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,6 @@ Livox ROS Driver 2 of current version support only ROS1 (noetic recommended), an ### 1.1 Prerequisites * OS: Ubuntu 18.04/20.04 * ROS: Noetic Ninjemys (ROS2 would be supported in the near future) -* SDK: A dedicated Livox SDK (dedicatedly applied to Livox ROS Driver 2) ### 1.2 Install ROS For ROS Noetic installation, please refer to: @@ -18,8 +17,8 @@ For ROS Noetic installation, please refer to: Desktop-Full installation is recommend. -### 1.3 Install the dedicated Livox SDK -1. Clone Livox ROS Driver 2 source code from GitHub: +## 2. Build & run Livox ROS Driver 2 +1. Clone Livox ROS Driver 2 source code: ```shell git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 ``` @@ -28,23 +27,19 @@ git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ cd ws_livox/src/livox_ros_driver2 sudo ./dedicated_sdk/build.sh ROS1 ``` - - -## 2. Build & run Livox ROS Driver 2 -1. Change directory to the folder 'livox_ros_driver2' (as in the previous step). -2. Build the Livox ROS Driver 2 (take Noetic as example): +3. Build the Livox ROS Driver 2 (take Noetic as example): ```shell source /opt/ros/noetic/setup.sh ./build.sh ROS1 ``` -3. In the same terminal (and in the same directory) with the previous step, use the following commands to run Livox ROS Driver 2: +4. Run Livox ROS Driver 2: ```shell source ../../devel/setup.sh roslaunch livox_ros_driver2 [launch file] ``` -in which, -* livox_ros_driver2: the ROS package name of Livox ROS Driver 2; -* [launch file]: the ROS launch file in the 'launch_ROS1' folder, containing the launch info and config about the target LiDAR(s); +in which, +* livox_ros_driver2: the ROS package name of Livox ROS Driver 2; +* [launch file]: the ROS launch file in the 'launch_ROS1' folder, containing the launch info and config about the target LiDAR(s); A rviz launch example for HAP LiDAR would be: ```shell diff --git a/cmake/version.cmake b/cmake/version.cmake index 526ce5fc..908c19b2 100644 --- a/cmake/version.cmake +++ b/cmake/version.cmake @@ -1,16 +1,16 @@ #--------------------------------------------------------------------------------------- -# Get livox_ros_driver version from include/livox_ros_driver.h +# Get livox_ros_driver2 version from include/livox_ros_driver2.h #--------------------------------------------------------------------------------------- -file(READ "${CMAKE_CURRENT_LIST_DIR}/../src/include/livox_ros_driver.h" LIVOX_ROS_DRIVER_VERSION_FILE) -string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MAJOR ([0-9]+)" _ "${LIVOX_ROS_DRIVER_VERSION_FILE}") +file(READ "${CMAKE_CURRENT_LIST_DIR}/../src/include/livox_ros_driver2.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_DRIVER_VERSION_FILE}") +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_DRIVER_VERSION_FILE}") +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_ros_driver.h") + message(FATAL_ERROR "Could not extract valid version from include/livox_ros_driver2.h") endif() set (LIVOX_ROS_DRIVER2_VERSION "${ver_major}.${ver_minor}.${ver_patch}") diff --git a/config_ROS1/HAP_config.json b/config_ROS1/HAP_config.json index b8034fe9..bdf21827 100644 --- a/config_ROS1/HAP_config.json +++ b/config_ROS1/HAP_config.json @@ -3,8 +3,6 @@ "lidar_type": 8 }, "HAP": { - "device_type" : "HAP", - "lidar_ipaddr": "", "lidar_net_info" : { "cmd_data_port": 56000, "push_msg_port": 0, @@ -32,12 +30,12 @@ "pattern_mode" : 0, "blind_spot_set" : 50, "extrinsic_parameter" : { - "roll": 1.1, - "pitch": 2.2, - "yaw": 3.3, - "x": 4, - "y": 5, - "z": 6 + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "x": 0, + "y": 0, + "z": 0 } } ] diff --git a/dedicated_sdk/CMakeLists.txt b/dedicated_sdk/CMakeLists.txt index 216d36bc..4a74b39b 100644 --- a/dedicated_sdk/CMakeLists.txt +++ b/dedicated_sdk/CMakeLists.txt @@ -17,38 +17,26 @@ if (UNIX) endif(UNIX) add_subdirectory(livox_sdk/sdk_core sdk_core) - add_subdirectory(livox_sdk_vehicle/sdk_core vehicle_sdk_core) - add_subdirectory(livox_lidar_sdk/sdk_core livox_sdk_core) - -add_subdirectory(livox_sdk_direct/sdk_core direct_sdk_core) add_subdirectory(livox_sdk_common) - - -add_subdirectory(sample/industry_lidar_sample) -add_subdirectory(sample/vehicle_lidar_quick_start) -add_subdirectory(sample/vehicle_lidar_preconfig) -add_subdirectory(sample/parse_lvx_file) -add_subdirectory(sample/record_lvx2_file) -add_subdirectory(sample/record_lvx2_file_industrial) -add_subdirectory(sample/vehicle_lidar_upgrade) -add_subdirectory(sample/industry_lidar_upgrade) -add_subdirectory(sample/obstacle_avoidance_test) -add_subdirectory(sample/direct_custom_lidar) -add_subdirectory(sample/direct_lidar) -add_subdirectory(sample/direct_lidars_upgrade) -add_subdirectory(sample/direct_lidar_update) -add_subdirectory(sample/direct_lidar_reset) - -add_subdirectory(sample/vehicle_multi_lidar_upgrade) - -add_subdirectory(sample/livox_multi_lidar_upgrade) -add_subdirectory(sample/livox_lidar_quick_start) -add_subdirectory(sample/livox_lidar_view_quick_start) -add_subdirectory(sample/livox_lidar_for_test) -add_subdirectory(sample/livox_lidar_record_lvx2_file) -add_subdirectory(sample/livox_lidar_parse_lvx2_file) - - +add_subdirectory(sample/industry/industry_lidar_sample) +add_subdirectory(sample/industry/industry_lidar_upgrade) + +add_subdirectory(sample/lvx/parse_lvx_file) +add_subdirectory(sample/lvx/record_lvx2_file) +add_subdirectory(sample/lvx/record_lvx2_file_industrial) +add_subdirectory(sample/lvx/obstacle_avoidance_test) + +add_subdirectory(sample/vehicle/vehicle_multi_lidar_upgrade) +add_subdirectory(sample/vehicle/vehicle_lidar_upgrade) +add_subdirectory(sample/vehicle/vehicle_lidar_quick_start) +add_subdirectory(sample/vehicle/vehicle_lidar_preconfig) + +add_subdirectory(sample/hap/livox_multi_lidar_upgrade) +add_subdirectory(sample/hap/livox_lidar_quick_start) +add_subdirectory(sample/hap/livox_lidar_view_quick_start) +add_subdirectory(sample/hap/livox_lidar_for_test) +add_subdirectory(sample/hap/livox_lidar_record_lvx2_file) +add_subdirectory(sample/hap/livox_lidar_parse_lvx2_file) \ No newline at end of file diff --git a/dedicated_sdk/CMakeLists.txt.shared b/dedicated_sdk/CMakeLists.txt.shared index 6481c7e0..4f681df8 100644 --- a/dedicated_sdk/CMakeLists.txt.shared +++ b/dedicated_sdk/CMakeLists.txt.shared @@ -19,7 +19,6 @@ endif(UNIX) add_subdirectory(livox_sdk/sdk_core sdk_core) add_subdirectory(livox_sdk_vehicle/sdk_core vehicle_sdk_core) -add_subdirectory(livox_sdk_direct/sdk_core direct_sdk_core) add_subdirectory(livox_lidar_sdk/sdk_core livox_lidar_sdk_core) add_subdirectory(livox_sdk_common) diff --git a/dedicated_sdk/CMakeLists.txt.static b/dedicated_sdk/CMakeLists.txt.static index 216d36bc..4a74b39b 100644 --- a/dedicated_sdk/CMakeLists.txt.static +++ b/dedicated_sdk/CMakeLists.txt.static @@ -17,38 +17,26 @@ if (UNIX) endif(UNIX) add_subdirectory(livox_sdk/sdk_core sdk_core) - add_subdirectory(livox_sdk_vehicle/sdk_core vehicle_sdk_core) - add_subdirectory(livox_lidar_sdk/sdk_core livox_sdk_core) - -add_subdirectory(livox_sdk_direct/sdk_core direct_sdk_core) add_subdirectory(livox_sdk_common) - - -add_subdirectory(sample/industry_lidar_sample) -add_subdirectory(sample/vehicle_lidar_quick_start) -add_subdirectory(sample/vehicle_lidar_preconfig) -add_subdirectory(sample/parse_lvx_file) -add_subdirectory(sample/record_lvx2_file) -add_subdirectory(sample/record_lvx2_file_industrial) -add_subdirectory(sample/vehicle_lidar_upgrade) -add_subdirectory(sample/industry_lidar_upgrade) -add_subdirectory(sample/obstacle_avoidance_test) -add_subdirectory(sample/direct_custom_lidar) -add_subdirectory(sample/direct_lidar) -add_subdirectory(sample/direct_lidars_upgrade) -add_subdirectory(sample/direct_lidar_update) -add_subdirectory(sample/direct_lidar_reset) - -add_subdirectory(sample/vehicle_multi_lidar_upgrade) - -add_subdirectory(sample/livox_multi_lidar_upgrade) -add_subdirectory(sample/livox_lidar_quick_start) -add_subdirectory(sample/livox_lidar_view_quick_start) -add_subdirectory(sample/livox_lidar_for_test) -add_subdirectory(sample/livox_lidar_record_lvx2_file) -add_subdirectory(sample/livox_lidar_parse_lvx2_file) - - +add_subdirectory(sample/industry/industry_lidar_sample) +add_subdirectory(sample/industry/industry_lidar_upgrade) + +add_subdirectory(sample/lvx/parse_lvx_file) +add_subdirectory(sample/lvx/record_lvx2_file) +add_subdirectory(sample/lvx/record_lvx2_file_industrial) +add_subdirectory(sample/lvx/obstacle_avoidance_test) + +add_subdirectory(sample/vehicle/vehicle_multi_lidar_upgrade) +add_subdirectory(sample/vehicle/vehicle_lidar_upgrade) +add_subdirectory(sample/vehicle/vehicle_lidar_quick_start) +add_subdirectory(sample/vehicle/vehicle_lidar_preconfig) + +add_subdirectory(sample/hap/livox_multi_lidar_upgrade) +add_subdirectory(sample/hap/livox_lidar_quick_start) +add_subdirectory(sample/hap/livox_lidar_view_quick_start) +add_subdirectory(sample/hap/livox_lidar_for_test) +add_subdirectory(sample/hap/livox_lidar_record_lvx2_file) +add_subdirectory(sample/hap/livox_lidar_parse_lvx2_file) \ No newline at end of file diff --git a/dedicated_sdk/build.sh b/dedicated_sdk/build.sh index 03a04c26..46726330 100755 --- a/dedicated_sdk/build.sh +++ b/dedicated_sdk/build.sh @@ -26,8 +26,6 @@ function CheckParams() { # rm /usr/local/lib/liblivox_sdk_common_static.a # fi -# if [ -f "/usr/local/lib/liblivox_sdk_direct_static.a"]; then -# rm /usr/local/lib/liblivox_sdk_direct_static.a # fi # if [ -f "/usr/local/lib/liblivox_sdk_static.a"]; then # rm /usr/local/lib/liblivox_sdk_static.a @@ -45,7 +43,6 @@ function GenerateCmakeFile() { cp ./CMakeLists.txt.static ./CMakeLists.txt cp ./livox_sdk/sdk_core/CMakeLists.txt.static ./livox_sdk/sdk_core/CMakeLists.txt cp ./livox_sdk_common/CMakeLists.txt.static ./livox_sdk_common/CMakeLists.txt - cp ./livox_sdk_direct/sdk_core/CMakeLists.txt.static ./livox_sdk_direct/sdk_core/CMakeLists.txt cp ./livox_sdk_vehicle/sdk_core/CMakeLists.txt.static ./livox_sdk_vehicle/sdk_core/CMakeLists.txt cp ./livox_lidar_sdk/sdk_core/CMakeLists.txt.static ./livox_lidar_sdk/sdk_core/CMakeLists.txt @@ -53,7 +50,6 @@ function GenerateCmakeFile() { cp ./CMakeLists.txt.shared ./CMakeLists.txt cp ./livox_sdk/sdk_core/CMakeLists.txt.shared ./livox_sdk/sdk_core/CMakeLists.txt cp ./livox_sdk_common/CMakeLists.txt.shared ./livox_sdk_common/CMakeLists.txt - cp ./livox_sdk_direct/sdk_core/CMakeLists.txt.shared ./livox_sdk_direct/sdk_core/CMakeLists.txt cp ./livox_sdk_vehicle/sdk_core/CMakeLists.txt.shared ./livox_sdk_vehicle/sdk_core/CMakeLists.txt cp ./livox_lidar_sdk/sdk_core/CMakeLists.txt.shared ./livox_lidar_sdk/sdk_core/CMakeLists.txt fi @@ -63,7 +59,8 @@ function Complier() { if [ ! -d "./build" ];then mkdir build else - echo "文件夹已经存在" + rm build -R + mkdir build fi cd ./build diff --git a/dedicated_sdk/livox_lidar_sdk/LICENSE.txt b/dedicated_sdk/livox_lidar_sdk/LICENSE.txt index d64b4ea7..678b41a2 100644 --- a/dedicated_sdk/livox_lidar_sdk/LICENSE.txt +++ b/dedicated_sdk/livox_lidar_sdk/LICENSE.txt @@ -47,8 +47,8 @@ Livox-SDK │ │ ├── comm_port.h │ │ └── protocol.h │ ├── config.h - │ ├── livox_direct_def.h - │ └─ livox_direct_sdk.h + │ ├── livox_lidar_def.h + │ └─ livox_lidar_api.h └── src ├── base │ ├── command_callback.h diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt b/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt index 96c3ca9f..15b6de86 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt @@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 3.0) set(SDK_LIBRARY livox_lidar_sdk_static) - add_library(${SDK_LIBRARY} STATIC "") set(LIVOX_SDK_MAJOR_VERSION "2") @@ -10,7 +9,6 @@ set(LIVOX_SDK_MINOR_VERSION "3") set(LIVOX_SDK_PATCH_VERSION "0") set(LIVOX_SDK_VERSION_STRING "${LIVOX_SDK_MAJOR_VERSION}.${LIVOX_SDK_MINOR_VERSION}.${LIVOX_SDK_PATCH_VERSION}") - target_include_directories(${SDK_LIBRARY} PUBLIC include @@ -22,7 +20,7 @@ target_include_directories(${SDK_LIBRARY} PRIVATE src) -set_target_properties(${SDK_LIBRARY} PROPERTIES PUBLIC_HEADER "include/livox_lidar_def.h;include/livox_lidar_sdk.h") +set_target_properties(${SDK_LIBRARY} PROPERTIES PUBLIC_HEADER "include/livox_lidar_def.h;include/livox_lidar_api.h") if(WIN32) set(PLATFORM win) @@ -59,7 +57,6 @@ target_sources(${SDK_LIBRARY} src/device_manager.h src/device_manager.cpp - src/comm/comm_port.cpp src/comm/sdk_protocol.h src/comm/sdk_protocol.cpp @@ -76,12 +73,22 @@ target_sources(${SDK_LIBRARY} src/data_handler/data_handler.cpp src/command_handler/command_handler.h + src/command_handler/command_impl.h + src/command_handler/command_impl.cpp src/command_handler/general_command_handler.h src/command_handler/general_command_handler.cpp src/command_handler/hap_command_handler.h src/command_handler/hap_command_handler.cpp + src/command_handler/mid360_command_handler.h + src/command_handler/mid360_command_handler.cpp + src/command_handler/pa_command_handler.h + src/command_handler/pa_command_handler.cpp + src/command_handler/build_request.h src/command_handler/build_request.cpp + + src/command_handler/parse_lidar_state_info.h + src/command_handler/parse_lidar_state_info.cpp src/base/multiple_io/multiple_io_base.h src/base/multiple_io/multiple_io_base.cpp @@ -97,7 +104,6 @@ target_sources(${SDK_LIBRARY} src/base/wake_up/wake_up_pipe.h src/base/wake_up/${PLATFORM}/wake_up_pipe.cpp) - install(TARGETS ${SDK_LIBRARY} PUBLIC_HEADER DESTINATION include ARCHIVE DESTINATION lib diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt.shared b/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt.shared index e44654c1..064a8a96 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt.shared +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt.shared @@ -21,7 +21,7 @@ target_include_directories(${SDK_LIBRARY} PRIVATE src) -set_target_properties(${SDK_LIBRARY} PROPERTIES PUBLIC_HEADER "include/livox_lidar_def.h;include/livox_lidar_sdk.h") +set_target_properties(${SDK_LIBRARY} PROPERTIES PUBLIC_HEADER "include/livox_lidar_def.h;include/livox_lidar_api.h") if(WIN32) set(PLATFORM win) @@ -34,7 +34,6 @@ target_compile_options(${SDK_LIBRARY} PRIVATE $<$:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long> PRIVATE $<$:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long>) - target_sources(${SDK_LIBRARY} PRIVATE src/third_party/FastCRC/FastCRC_tables.hpp @@ -59,7 +58,6 @@ target_sources(${SDK_LIBRARY} src/device_manager.h src/device_manager.cpp - src/comm/comm_port.cpp src/comm/sdk_protocol.h src/comm/sdk_protocol.cpp @@ -76,12 +74,22 @@ target_sources(${SDK_LIBRARY} src/data_handler/data_handler.cpp src/command_handler/command_handler.h + src/command_handler/command_impl.h + src/command_handler/command_impl.cpp src/command_handler/general_command_handler.h src/command_handler/general_command_handler.cpp src/command_handler/hap_command_handler.h src/command_handler/hap_command_handler.cpp + src/command_handler/mid360_command_handler.h + src/command_handler/mid360_command_handler.cpp + src/command_handler/pa_command_handler.h + src/command_handler/pa_command_handler.cpp + src/command_handler/build_request.h src/command_handler/build_request.cpp + + src/command_handler/parse_lidar_state_info.h + src/command_handler/parse_lidar_state_info.cpp src/base/multiple_io/multiple_io_base.h src/base/multiple_io/multiple_io_base.cpp @@ -97,7 +105,6 @@ target_sources(${SDK_LIBRARY} src/base/wake_up/wake_up_pipe.h src/base/wake_up/${PLATFORM}/wake_up_pipe.cpp) - install(TARGETS ${SDK_LIBRARY} PUBLIC_HEADER DESTINATION include ARCHIVE DESTINATION lib diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt.static b/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt.static index 96c3ca9f..15b6de86 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt.static +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/CMakeLists.txt.static @@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 3.0) set(SDK_LIBRARY livox_lidar_sdk_static) - add_library(${SDK_LIBRARY} STATIC "") set(LIVOX_SDK_MAJOR_VERSION "2") @@ -10,7 +9,6 @@ set(LIVOX_SDK_MINOR_VERSION "3") set(LIVOX_SDK_PATCH_VERSION "0") set(LIVOX_SDK_VERSION_STRING "${LIVOX_SDK_MAJOR_VERSION}.${LIVOX_SDK_MINOR_VERSION}.${LIVOX_SDK_PATCH_VERSION}") - target_include_directories(${SDK_LIBRARY} PUBLIC include @@ -22,7 +20,7 @@ target_include_directories(${SDK_LIBRARY} PRIVATE src) -set_target_properties(${SDK_LIBRARY} PROPERTIES PUBLIC_HEADER "include/livox_lidar_def.h;include/livox_lidar_sdk.h") +set_target_properties(${SDK_LIBRARY} PROPERTIES PUBLIC_HEADER "include/livox_lidar_def.h;include/livox_lidar_api.h") if(WIN32) set(PLATFORM win) @@ -59,7 +57,6 @@ target_sources(${SDK_LIBRARY} src/device_manager.h src/device_manager.cpp - src/comm/comm_port.cpp src/comm/sdk_protocol.h src/comm/sdk_protocol.cpp @@ -76,12 +73,22 @@ target_sources(${SDK_LIBRARY} src/data_handler/data_handler.cpp src/command_handler/command_handler.h + src/command_handler/command_impl.h + src/command_handler/command_impl.cpp src/command_handler/general_command_handler.h src/command_handler/general_command_handler.cpp src/command_handler/hap_command_handler.h src/command_handler/hap_command_handler.cpp + src/command_handler/mid360_command_handler.h + src/command_handler/mid360_command_handler.cpp + src/command_handler/pa_command_handler.h + src/command_handler/pa_command_handler.cpp + src/command_handler/build_request.h src/command_handler/build_request.cpp + + src/command_handler/parse_lidar_state_info.h + src/command_handler/parse_lidar_state_info.cpp src/base/multiple_io/multiple_io_base.h src/base/multiple_io/multiple_io_base.cpp @@ -97,7 +104,6 @@ target_sources(${SDK_LIBRARY} src/base/wake_up/wake_up_pipe.h src/base/wake_up/${PLATFORM}/wake_up_pipe.cpp) - install(TARGETS ${SDK_LIBRARY} PUBLIC_HEADER DESTINATION include ARCHIVE DESTINATION lib diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_api.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_api.h new file mode 100644 index 00000000..bcf86c04 --- /dev/null +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_api.h @@ -0,0 +1,491 @@ +// +// 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. +// + +#ifndef LIVOX_LIDAR_SDK_H_ +#define LIVOX_LIDAR_SDK_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "livox_lidar_def.h" + +/** +* Return SDK's version information in a numeric form. +* @param version Pointer to a version structure for returning the version information. +*/ +void GetLivoxLidarSdkVer(LivoxLidarSdkVer *version); + +/** + * Initialize the SDK. + * @return true if successfully initialized, otherwise false. + */ +bool LivoxLidarSdkInit(const char* path, const char* host_ip = ""); + +/** + * Start the device scanning routine which runs on a separate thread. + * @return true if successfully started, otherwise false. + */ +bool LivoxLidarSdkStart(); + +/** + * Uninitialize the SDK. + */ +void LivoxLidarSdkUninit(); + +/** + * Callback function for receiving point cloud data. + * + * @param handle device handle. + * @param data device's data. + * @param data_num number of points in data. + * @param client_data user data associated with the command. + */ +typedef void (*LivoxLidarPointCloudCallBack)(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data); + +/** + * Set the callback to receive point cloud data. + * @param handle device handle. + * @param client_data user data associated with the command. + */ +void SetLivoxLidarPointCloudCallBack(LivoxLidarPointCloudCallBack cb, void* client_data); + +/** + * Callback function for point cloud observer. + * @param handle device handle. + * @param data device's data. + * @param data_num number of points in data. + * @param client_data user data associated with the command. + */ +typedef void (*LivoxLidarPointCloudObserver) (uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket *data, void *client_data); + +/** + * Set the point cloud observer. + * @param cb callback to receive Status Info. + * @param client_data user data associated with the command. + */ +uint16_t LivoxLidarAddPointCloudObserver(LivoxLidarPointCloudObserver cb, void *client_data); + +/** + * remove point cloud observer. + * @param id the observer id. + */ +void LivoxLidarRemovePointCloudObserver(uint16_t id); + +/** + * Callback function for receiving IMU data. + * @param data device's data. + * @param data_num number of IMU data. + * @param client_data user data associated with the command. + */ +typedef void (*LivoxLidarImuDataCallback)(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data); + +/** + * Set the callback to receive IMU data. + * @param cb callback to receive IMU data. + * @param client_data user data associated with the command. + */ +void SetLivoxLidarImuDataCallback(LivoxLidarImuDataCallback cb, void* client_data); + +/** + * Callback function for receiving Status Info. + * @param status_info status info. + * @param client_data user data associated with the command. + */ +typedef void(*LivoxLidarInfoCallback)(const uint32_t handle, const uint8_t dev_type, const char* info, void* client_data); + +/** + * Set the callback to receive Status Info. + * @param cb callback to receive Status Info. + * @param client_data user data associated with the command. + */ +void SetLivoxLidarInfoCallback(LivoxLidarInfoCallback cb, void* client_data); + +/** + * DisableLivoxLidar console log output. + */ +void DisableLivoxSdkConsoleLogger(); + +/** +* Save the log file. +*/ +void SaveLivoxLidarSdkLoggerFile(); + +/** + * Callback function for receiving Status Info. + * @param status_info status info. + * @param client_data user data associated with the command. + */ +typedef void(*LivoxLidarInfoChangeCallback)(const uint32_t handle, const LivoxLidarInfo* info, void* client_data); + +/** + * Set the callback to receive Status Info. + * @param cb callback to receive Status Info. + * @param client_data user data associated with the command. + */ +void SetLivoxLidarInfoChangeCallback(LivoxLidarInfoChangeCallback cb, void* client_data); + +typedef void (*QueryLivoxLidarInternalInfoCallback)(livox_status status, uint32_t handle, + LivoxLidarDiagInternalInfoResponse* response, void* client_data); +livox_status QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data); + +typedef void (*LivoxLidarAsyncControlCallback)(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, void *client_data); + + +/** + * Set LiDAR pcl data type. + * @param handle device handle. + * @param data_type the type to change, the val:kLivoxLidarCartesianCoordinateHighData, + * kLivoxLidarCartesianCoordinateLowData, + * kLivoxLidarSphericalCoordinateData + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR sacn pattern. + * @param handle device handle. + * @param scan_type the type to change, the val:kLivoxLidarScanPatternRepetive, + * kLivoxLidarScanPatternNoneRepetive + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR dual emit. + * @param handle device handle. + * @param enable if set dual emit the enable is true, else the enable is false + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Embale LiDAR point send. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Disable LiDAR point send. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR Ip info. + * @param handle device handle. + * @param ipconfig lidar ip info. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarIp(uint32_t handle, LivoxLidarIpInfo* ip_config, + LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR state Ip info. + * @param handle device handle. + * @param host_state_info_ipcfg lidar ip info. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarStateInfoHostIPCfg(uint32_t handle, HostStateInfoIpInfo* host_state_info_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR point cloud host ip info. + * @param handle device handle. + * @param host_point_ipcfg lidar ip info. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarPointDataHostIPCfg(uint32_t handle, HostPointIPInfo* host_point_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR imu data host ip info. + * @param handle device handle. + * @param host_imu_ipcfg lidar ip info. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarImuDataHostIPCfg(uint32_t handle, HostImuDataIPInfo* host_imu_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Get LiDAR extrinsic parameters. + * @param handle device handle. + * @param install_attitude extrinsic parameters. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarInstallAttitude(uint32_t handle, LivoxLidarInstallAttitude* install_attitude, + LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR roi cfg0. + * @param handle device handle. + * @param roi_cfg0 roi cfg. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarRoiCfg0(uint32_t handle, RoiCfg* roi_cfg0, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR roi. + * @param handle device handle. + * @param roi_cfg1 roi cfg. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarRoiCfg1(uint32_t handle, RoiCfg* roi_cfg1, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Enable LiDAR roi cfg1. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status EnableLivoxLidarRoi(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Disable LiDAR roi. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status DisableLivoxLidarRoi(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR detect mode. + * @param handle device handle. + * @param mode detect mode + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarDetectMode(uint32_t handle, LivoxLidarDetectMode mode, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR func io cfg. + * @param handle device handle. + * @param func_io_cfg func io cfg; 0:IN0,1:IN1, 2:OUT0, 3:OUT1;Mid360 lidar 8, 10, 12, 11 + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarFuncIOCfg(uint32_t handle, FuncIOCfg* func_io_cfg, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR blind spot. + * @param handle device handle. + * @param blind_spot blind spot. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR work mode. + * @param handle device handle. + * @param work_mode lidar work mode. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Enable LiDAR glass heat. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + + +/** + * Disable LiDAR glass heat. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Set LiDAR glass heat. + * @param handle device handle. + * @param glass_heat lidar glass heat. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Enable LiDAR imu data. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +/** + * Disable LiDAR imu data. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +// mid360 lidar does not support +/** + * Enable LiDAR fusa function. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +// mid360 lidar does not support +/** + * Enable LiDAR fusa function. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + +typedef void (*LivoxLidarResetCallback)(livox_status status, uint32_t handle, LivoxLidarResetResponse* response, void* client_data); +/** + * Reset LiDAR. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data); + +/*******Upgrade Module***********/ +typedef void (*LivoxLidarRebootCallback)(livox_status status, uint32_t handle, LivoxLidarRebootResponse* response, void* client_data); +livox_status LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, void* client_data); + + /** + * Upgrade related command + */ +typedef void (*LivoxLidarStartUpgradeCallback)(livox_status status, uint32_t handle, + LivoxLidarStartUpgradeResponse* response, void* client_data); +/** + * LiDAR start upgrade + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length, + LivoxLidarStartUpgradeCallback cb, void* client_data); + +typedef void (*LivoxLidarXferFirmwareCallback)(livox_status status, uint32_t handle, + LivoxLidarXferFirmwareResponse* response, void* client_data); +/** + * Transfer LiDAR firmware. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, + LivoxLidarXferFirmwareCallback cb, void* client_data); + +typedef void (*LivoxLidarCompleteXferFirmwareCallback)(livox_status status, uint32_t handle, + LivoxLidarCompleteXferFirmwareResponse* response, void* client_data); +/** + * Verify the integrity of the LiDAR transmission firmware. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data, + uint16_t length, LivoxLidarCompleteXferFirmwareCallback cb, void* client_data); + +typedef void (*LivoxLidarGetUpgradeProgressCallback)(livox_status status, + uint32_t handle, LivoxLidarGetUpgradeProgressResponse* response, void* client_data); +/** + * Get LiDAR upgrade progress. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data, + uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data); + +typedef void (*LivoxLidarRequestFirmwareInfoCallback)(livox_status status, + uint32_t handle, LivoxLidarRequestFirmwareInfoResponse* response, void* client_data); +/** + * Query LiDAR firmware info. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error code. + */ +livox_status LivoxLidarRequestFirmwareInfo(uint32_t handle, + LivoxLidarRequestFirmwareInfoCallback cb, void* client_data); + +#ifdef __cplusplus +} +#endif + +#endif // LIVOX_LIDAR_SDK_H_ diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_cfg.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_cfg.h index a2beecd0..02375e89 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_cfg.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_cfg.h @@ -22,8 +22,8 @@ // SOFTWARE. // -#ifndef LIVOX_CONFIG_DIRECT_H_ -#define LIVOX_CONFIG_DIRECT_H_ +#ifndef LIVOX_LIDAR_CONFIG_H_ +#define LIVOX_LIDAR_CONFIG_H_ #if defined(__linux__) #include diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_def.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_def.h index e6c33d18..aa1b5b64 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_def.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_def.h @@ -54,18 +54,41 @@ typedef struct { char lidar_ip[16]; } LivoxLidarInfo; +/** Device type. */ +typedef enum { + kLivoxLidarTypeHub = 0, + kLivoxLidarTypeMid40 = 1, + kLivoxLidarTypeTele = 2, + kLivoxLidarTypeHorizon = 3, + kLivoxLidarTypeMid70 = 6, + kLivoxLidarTypeAvia = 7, + kLivoxLidarTypeMid360 = 9, + kLivoxLidarTypeIndustrialHAP = 10, + kLivoxLidarTypeHAP = 15, + kLivoxLidarTypePA = 16, +} LivoxLidarDeviceType; + typedef enum { kKeyPclDataType = 0x0000, kKeyPatternMode = 0x0001, kKeyDualEmitEnable = 0x0002, kKeyPointSendEnable = 0x0003, kKeyLidarIPCfg = 0x0004, - + kKeyStateInfoHostIPCfg = 0x0005, + + kKeyLidarPointDataHostIPCfg = 0x0006, kKeyLidarImuHostIPCfg = 0x0007, kKeyInstallAttitude = 0x0012, kKeyBlindSpotSet = 0x0013, + + kKeyRoiCfg0 = 0x0015, + kKeyRoiCfg1 = 0x0016, + + kKeyRoiEn = 0x0017, + kKeyDetectMode = 0x0018, + kKeyFuncIOCfg = 0x0019, kKeyWorkMode = 0x001A, kKeyGlassHeat = 0x001B, @@ -81,10 +104,19 @@ typedef enum { kKeyVersionHardware = 0x8004, kKeyMac = 0x8005, kKeyCurWorkState = 0x8006, - + kKeyCoreTemp = 0x8007, + kKeyPowerUpCnt = 0x8008, + kKeyLocalTimeNow = 0x8009, + kKeyLastSyncTime = 0x800A, + kKeyTimeOffset = 0x800B, + kKeyTimeSyncType = 0x800C, + kKeyStatusCode = 0x800D, kKeyLidarDiagStatus = 0x800E, kKeyLidarFlashStatus = 0x800F, + kKeyHmsCode = 0x8011, + kKeyFwType = 0x8010, + kKeyLidarDiagInfoQuery = 0xFFFF } ParamKeyName; @@ -167,6 +199,11 @@ typedef struct { uint16_t error_key; } LivoxLidarAsyncControlResponse; +typedef struct { + uint8_t ret_code; + const char* lidar_info; +} LivoxLidarInfoResponse; + typedef struct { uint8_t ret_code; uint16_t param_num; @@ -174,8 +211,9 @@ typedef struct { } LivoxLidarDiagInternalInfoResponse; typedef enum { - kLivoxLidarScanPatternRepetive = 0x00, - kLivoxLidarScanPatternNoneRepetive = 0x01 + kLivoxLidarScanPatternNoneRepetive = 0x00, + kLivoxLidarScanPatternRepetive = 0x01, + kLivoxLidarScanPatternRepetiveLowFrameRate = 0x02 } LivoxLidarScanPattern; typedef enum { @@ -205,12 +243,38 @@ typedef struct { int32_t z; // mm } LivoxLidarInstallAttitude; +typedef struct { + int32_t yaw_start; + int32_t yaw_stop; + int32_t pitch_start; + int32_t pitch_stop; + uint32_t rsvd; +} RoiCfg; + +typedef enum { + kLivoxLidarDetectNormal = 0x00, + kLivoxLidarDetectSensitive = 0x01 +} LivoxLidarDetectMode; + +typedef struct { + uint8_t in0; + uint8_t int1; + uint8_t out0; + uint8_t out1; +} FuncIOCfg; + typedef struct { char ip_addr[16]; /**< IP address. */ char net_mask[16]; /**< Subnet mask. */ char gw_addr[16]; /**< Gateway address. */ } LivoxLidarIpInfo; +typedef struct { + char host_ip_addr[16]; /**< IP address. */ + uint16_t host_state_info_port; + uint16_t lidar_state_info_port; +} HostStateInfoIpInfo; + typedef struct { char host_ip_addr[16]; /**< IP address. */ uint16_t host_point_data_port; @@ -247,6 +311,45 @@ typedef struct { uint64_t status_code; } LivoxLidarStateInfo; +typedef struct { + uint8_t pcl_data_type; + uint8_t pattern_mode; + + LivoxLidarIpInfo livox_lidar_ip_info; + HostStateInfoIpInfo host_state_info; + HostPointIPInfo host_point_ip_info; + HostImuDataIPInfo host_imu_data_ip_info; + LivoxLidarInstallAttitude install_attitude; + + RoiCfg roi_cfg0; + RoiCfg roi_cfg1; + + uint8_t roi_en; + uint8_t work_mode; + uint8_t imu_data_en; + + char sn[16]; + char product_info[64]; + uint8_t version_app[4]; + uint8_t version_load[4]; + uint8_t version_hardware[4]; + uint8_t mac[6]; + + uint8_t cur_work_state; + int32_t core_temp; + uint32_t powerup_cnt; + + uint64_t local_time_now; + uint64_t last_sync_time; + int64_t time_offset; + + uint8_t time_sync_type; + + uint16_t diag_status; + uint8_t fw_type; + uint32_t hms_code[8]; +} DirectLidarStateInfo; + typedef struct { uint8_t ret_code; LivoxLidarStateInfo livox_lidar_state_info; @@ -335,7 +438,7 @@ typedef struct { } LivoxLidarRebootResponse; typedef struct { - uint16_t data; + uint8_t data[16]; } LivoxLidarResetRequest; typedef struct { diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_sdk.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_sdk.h deleted file mode 100644 index c5b025f8..00000000 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/include/livox_lidar_sdk.h +++ /dev/null @@ -1,224 +0,0 @@ -// -// 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. -// - -#ifndef LIVOX_LIDAR_SDK_H_ -#define LIVOX_LIDAR_SDK_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include "livox_lidar_def.h" - -/** -* Return SDK's version information in a numeric form. -* @param version Pointer to a version structure for returning the version information. -*/ -void GetLivoxLidarSdkVer(LivoxLidarSdkVer *version); - -/** - * Initialize the SDK. - * @return true if successfully initialized, otherwise false. - */ -bool LivoxLidarSdkInit(const char* path, const char* host_ip = ""); - -/** - * Start the device scanning routine which runs on a separate thread. - * @return true if successfully started, otherwise false. - */ -bool LivoxLidarSdkStart(); - -/** - * Uninitialize the SDK. - */ -void LivoxLidarSdkUninit(); - -/** - * Callback function for receiving point cloud data. - * - * @param slot device slot. - * @param data device's data. - * @param data_num number of points in data. - * @param client_data user data associated with the command. - */ -typedef void (*LivoxLidarPointCloudCallBack)(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data); - -/** - * Set the callback to receive point cloud data. - * @param slot device slot. - * @param client_data user data associated with the command. - */ -void SetLivoxLidarPointCloudCallBack(LivoxLidarPointCloudCallBack cb, void* client_data); - -/** - * Callback function for point cloud observer. - * @param slot device slot. - * @param data device's data. - * @param data_num number of points in data. - * @param client_data user data associated with the command. - */ -typedef void (*LivoxLidarPointCloudObserver) (uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket *data, void *client_data); - -/** - * Set the point cloud observer. - * @param cb callback to receive Status Info. - * @param client_data user data associated with the command. - */ -uint16_t LivoxLidarAddPointCloudObserver(LivoxLidarPointCloudObserver cb, void *client_data); - -/** - * remove point cloud observer. - * @param id the observer id. - */ -void LivoxLidarRemovePointCloudObserver(uint16_t id); - -/** - * Callback function for receiving IMU data. - * @param data device's data. - * @param data_num number of IMU data. - * @param client_data user data associated with the command. - */ -typedef void (*LivoxLidarImuDataCallback)(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data); - -/** - * Set the callback to receive IMU data. - * @param cb callback to receive IMU data. - * @param client_data user data associated with the command. - */ -void SetLivoxLidarImuDataCallback(LivoxLidarImuDataCallback cb, void* client_data); - - -/** - * DisableLivoxLidar console log output. - */ -void DisableLivoxSdkConsoleLogger(); - -/** -* Save the log file. -*/ -void SaveLivoxLidarSdkLoggerFile(); - -/** - * Callback function for receiving Status Info. - * @param status_info status info. - * @param client_data user data associated with the command. - */ -typedef void(*LivoxLidarInfoChangeCallback)(const uint32_t handle, const LivoxLidarInfo* info, void* client_data); - -/** - * Set the callback to receive Status Info. - * @param cb callback to receive Status Info. - * @param client_data user data associated with the command. - */ -void SetLivoxLidarInfoChangeCallback(LivoxLidarInfoChangeCallback cb, void* client_data); - -typedef void (*QueryLivoxLidarInternalInfoCallback)(livox_status status, uint32_t handle, - LivoxLidarDiagInternalInfoResponse* response, void* client_data); -livox_status QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data); - -typedef void (*LivoxLidarAsyncControlCallback)(livox_status status, uint32_t handle, - LivoxLidarAsyncControlResponse *response, void *client_data); - -/* - * kLivoxLidarCartesianCoordinateHighData, - * kLivoxLidarCartesianCoordinateLowData, - */ -livox_status SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); -livox_status DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status SetLivoxLidarIp(uint32_t handle, LivoxLidarIpInfo* ip_config, - LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status SetLivoxLidarPointDataHostIPCfg(uint32_t handle, HostPointIPInfo* host_point_ipcfg, - LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status SetLivoxLidarImuDataHostIPCfg(uint32_t handle, HostImuDataIPInfo* host_imu_ipcfg, - LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status SetLivoxLidarInstallAttitude(uint32_t handle, LivoxLidarInstallAttitude* install_attitude, - LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); -livox_status DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); -livox_status SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); -livox_status DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - -livox_status EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); -livox_status DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - -// reset lidar -typedef void (*LivoxLidarResetCallback)(livox_status status, uint32_t handle, LivoxLidarResetResponse* response, void* client_data); -livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data); - -/*******Upgrade Module***********/ -typedef void (*LivoxLidarRebootCallback)(livox_status status, uint32_t handle, LivoxLidarRebootResponse* response, void* client_data); -livox_status LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, void* client_data); - - /** - * Upgrade related command - */ -typedef void (*LivoxLidarStartUpgradeCallback)(livox_status status, uint32_t handle, - LivoxLidarStartUpgradeResponse* response, void* client_data); -livox_status LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length, - LivoxLidarStartUpgradeCallback cb, void* client_data); - -typedef void (*LivoxLidarXferFirmwareCallback)(livox_status status, uint32_t handle, - LivoxLidarXferFirmwareResponse* response, void* client_data); -livox_status LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, - LivoxLidarXferFirmwareCallback cb, void* client_data); - -typedef void (*LivoxLidarCompleteXferFirmwareCallback)(livox_status status, uint32_t handle, - LivoxLidarCompleteXferFirmwareResponse* response, void* client_data); -livox_status LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data, - uint16_t length, LivoxLidarCompleteXferFirmwareCallback cb, void* client_data); - -typedef void (*LivoxLidarGetUpgradeProgressCallback)(livox_status status, - uint32_t handle, LivoxLidarGetUpgradeProgressResponse* response, void* client_data); -livox_status LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data, - uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data); - -typedef void (*LivoxLidarRequestFirmwareInfoCallback)(livox_status status, - uint32_t handle, LivoxLidarRequestFirmwareInfoResponse* response, void* client_data); -livox_status LivoxLidarRequestFirmwareInfo(uint32_t handle, - LivoxLidarRequestFirmwareInfoCallback cb, void* client_data); - -#ifdef __cplusplus -} -#endif - -#endif // LIVOX_LIDAR_SDK_H_ diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/base/command_callback.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/base/command_callback.h index d9bda8c4..28cc2522 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/base/command_callback.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/base/command_callback.h @@ -27,7 +27,7 @@ #include #include -#include "livox_lidar_sdk.h" +#include "livox_lidar_api.h" namespace livox { namespace lidar { diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/comm/define.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/comm/define.h index a8f4b97c..8d328320 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/comm/define.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/comm/define.h @@ -80,20 +80,6 @@ typedef struct { GeneralCfgInfo general_cfg_info; } LivoxLidarCfg; -/** Device type. */ -typedef enum { - kLivoxLidarTypeHub = 0, - kLivoxLidarTypeMid40 = 1, - kLivoxLidarTypeTele = 2, - kLivoxLidarTypeHorizon = 3, - kLivoxLidarTypeMid70 = 6, - kLivoxLidarTypeAvia = 7, - kLivoxLidarTypeMid360 = 9, - kLivoxLidarTypeIndustrialHAP = 10, - kLivoxLidarTypeHAP = 15, - kLivoxLidarTypePA = 16 -} LivoxLidarDeviceType; - typedef enum { /** * Lidar command set, set the working mode and sub working mode of a LiDAR. @@ -181,6 +167,7 @@ typedef struct { static const uint16_t kDetectionPort = 56000; static const uint16_t kDetectionListenPort = 56001; + static const uint16_t kHAPCmdPort = 56000; static const uint16_t kHAPPointDataPort = 57000; static const uint16_t kHAPIMUPort = 58000; @@ -188,6 +175,24 @@ static const uint16_t kHAPLogPort = 59000; static const uint16_t kLogPort = 59000; +static const uint16_t kMid360LidarCmdPort = 56100; +static const uint16_t kMid360LidarPushMsgPort = 56200; +static const uint16_t kMid360LidarPointCloudPort = 56300; +static const uint16_t kMid360LidarImuDataPort = 56400; +static const uint16_t kMid360LidarLogPort = 56500; + +static const uint16_t kMid360HostCmdPort = 56101; +static const uint16_t kMid360HostPushMsgPort = 56201; +static const uint16_t kMid360HostPointCloudPort = 56301; +static const uint16_t kMid360HostImuDataPort = 56401; +static const uint16_t kMid360HostLogPort = 56501; + + +static const uint16_t kPaLidarCmdPort = 9347; +static const uint16_t kPaLidarPointCloudPort = 10000; +static const uint16_t kPaLidarFaultPort = 10001; +static const uint16_t kPaLidarLogPort = 1002; + typedef enum { kLidarCmdPort = 56100, diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/comm/sdk_protocol.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/comm/sdk_protocol.cpp index 1dfe6e28..7e72d7f4 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/comm/sdk_protocol.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/comm/sdk_protocol.cpp @@ -62,13 +62,19 @@ int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, sdk_packet->crc16_h = Crc_CalculateCRC16(o_buf, 18, 0, 1); - if (sdk_packet->length - GetPacketWrapperLen() == 0) { - - sdk_packet->crc32_d = 0xffffffff; - } else { + if (i_packet.data_len == 0) { + sdk_packet->crc32_d = 0; + } else { sdk_packet->crc32_d = Crc_CalculateCRC32(i_packet.data, i_packet.data_len, 0 , 1); } + // if (sdk_packet->length - GetPacketWrapperLen() == 0) { + + // sdk_packet->crc32_d = 0xffffffff; + // } else { + // sdk_packet->crc32_d = Crc_CalculateCRC32(i_packet.data, i_packet.data_len, 0 , 1); + // } + // sdk_packet->crc16_h = crc16_.mcrf4xx_calc(o_buf, 18); // sdk_packet->crc32_d = crc32_.crc32_calc(i_packet.data, i_packet.data_len); @@ -138,7 +144,7 @@ bool SdkProtocol::CheckPreamble(uint8_t *buf, uint32_t buf_size) { uint32_t crc32_d = 0; if (packet->length - GetPacketWrapperLen() == 0) { - crc32_d = 0xffffffff; + crc32_d = 0; } else { crc32_d = Crc_CalculateCRC32(packet->data, packet->length - GetPacketWrapperLen(), 0 , 1); } diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/build_request.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/build_request.cpp index 114c02b2..30977ef0 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/build_request.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/build_request.cpp @@ -47,41 +47,93 @@ bool BuildRequest::BuildUpdateViewLidarCfgRequest(const ViewLidarIpInfo& view_li point_kv->length = sizeof(uint8_t) * 8; HostIpInfoValue* host_point_ip_info_val = (HostIpInfoValue*)&point_kv->value; if (!InitHostIpAddr(view_lidar_info.host_ip, host_point_ip_info_val)) { + LOG_ERROR("Build update view lidar cfg request failed, init host ip addr failed."); return false; } - //const uint16_t lidar_point_port = kHAPPointDataPort; memcpy(&(host_point_ip_info_val->host_port), &view_lidar_info.host_point_port, sizeof(view_lidar_info.host_point_port)); memcpy(&(host_point_ip_info_val->lidar_port), &view_lidar_info.lidar_point_port, sizeof(view_lidar_info.lidar_point_port)); req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue); - - LivoxLidarKeyValueParam * imu_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len]; imu_kv->key = static_cast(kKeyLidarImuHostIPCfg); imu_kv->length = sizeof(uint8_t) * 8; HostIpInfoValue* host_imu_ip_info_val = (HostIpInfoValue*)&imu_kv->value; if (!InitHostIpAddr(view_lidar_info.host_ip, host_imu_ip_info_val)) { + LOG_ERROR("Build update view lidar cfg request failed, init imu host ip addr failed."); return false; } - //const uint16_t lidar_imu_port = kHAPIMUPort; - memcpy(&(host_point_ip_info_val->host_port), &view_lidar_info.host_imu_data_port, sizeof(view_lidar_info.host_imu_data_port)); - memcpy(&(host_point_ip_info_val->lidar_port), &view_lidar_info.lidar_imu_data_port, sizeof(view_lidar_info.lidar_imu_data_port)); + memcpy(&(host_imu_ip_info_val->host_port), &view_lidar_info.host_imu_data_port, sizeof(view_lidar_info.host_imu_data_port)); + memcpy(&(host_imu_ip_info_val->lidar_port), &view_lidar_info.lidar_imu_data_port, sizeof(view_lidar_info.lidar_imu_data_port)); req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue); return true; } +bool BuildRequest::BuildUpdateMid360LidarCfgRequest(const LivoxLidarCfg& lidar_cfg, + uint8_t* req_buf, uint16_t& req_len) { + + uint16_t key_num = 3; + memcpy(&req_buf[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + LivoxLidarKeyValueParam * state_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len]; + state_kv->key = static_cast(kKeyStateInfoHostIPCfg); + state_kv->length = sizeof(uint8_t) * 8; + HostIpInfoValue* host_state_ip_info_val = (HostIpInfoValue*)&state_kv->value; + if (!InitHostIpAddr(lidar_cfg.host_net_info.push_msg_ip, host_state_ip_info_val)) { + LOG_ERROR("Build update lidar cfg request failed, init host ip addr failed."); + return false; + } + uint16_t lidar_state_port = kMid360LidarPushMsgPort; + memcpy(&(host_state_ip_info_val->host_port), &lidar_cfg.host_net_info.push_msg_port, sizeof(lidar_cfg.host_net_info.push_msg_port)); + memcpy(&(host_state_ip_info_val->lidar_port), &lidar_state_port, sizeof(lidar_state_port)); + req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue); + LivoxLidarKeyValueParam * point_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len]; + point_kv->key = static_cast(kKeyLidarPointDataHostIPCfg); + point_kv->length = sizeof(uint8_t) * 8; + HostIpInfoValue* host_point_ip_info_val = (HostIpInfoValue*)&point_kv->value; + if (!InitHostIpAddr(lidar_cfg.host_net_info.point_data_ip, host_point_ip_info_val)) { + LOG_ERROR("Build update lidar cfg request failed, init host ip addr failed."); + return false; + } + uint16_t lidar_point_port = kMid360LidarPointCloudPort; + memcpy(&(host_point_ip_info_val->host_port), &lidar_cfg.host_net_info.point_data_port, sizeof(lidar_cfg.host_net_info.point_data_port)); + memcpy(&(host_point_ip_info_val->lidar_port), &lidar_point_port, sizeof(lidar_point_port)); + req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue); + LivoxLidarKeyValueParam * imu_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len]; + imu_kv->key = static_cast(kKeyLidarImuHostIPCfg); + imu_kv->length = sizeof(uint8_t) * 8; + HostIpInfoValue* host_imu_ip_info_val = (HostIpInfoValue*)&imu_kv->value; + if (!InitHostIpAddr(lidar_cfg.host_net_info.imu_data_ip, host_imu_ip_info_val)) { + LOG_ERROR("Build update lidar cfg request failed, init imu host ip addr failed."); + return false; + } + + uint16_t lidar_imu_port = kMid360LidarImuDataPort; + memcpy(&(host_imu_ip_info_val->host_port), &lidar_cfg.host_net_info.imu_data_port, sizeof(lidar_cfg.host_net_info.imu_data_port)); + memcpy(&(host_imu_ip_info_val->lidar_port), &lidar_imu_port, sizeof(lidar_imu_port)); + req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue); + // LOG_ERROR("Build imu host ip:{}, host_port:{}, lidar_port:{}", lidar_cfg.host_net_info.imu_data_ip.c_str(), + // lidar_cfg.host_net_info.imu_data_port, lidar_imu_port); + return true; +} bool BuildRequest::BuildUpdateLidarCfgRequest(const LivoxLidarCfg& lidar_cfg, uint8_t* req_buf, uint16_t& req_len) { - uint16_t key_num = 2; + + uint16_t key_num = 0; + if(lidar_cfg.device_type == kLivoxLidarTypePA) { + key_num = 1; + } else { + key_num = 2; + } + memcpy(&req_buf[req_len], &key_num, sizeof(key_num)); req_len = sizeof(key_num) + sizeof(uint16_t); @@ -90,28 +142,53 @@ bool BuildRequest::BuildUpdateLidarCfgRequest(const LivoxLidarCfg& lidar_cfg, point_kv->length = sizeof(uint8_t) * 8; HostIpInfoValue* host_point_ip_info_val = (HostIpInfoValue*)&point_kv->value; if (!InitHostIpAddr(lidar_cfg.host_net_info.point_data_ip, host_point_ip_info_val)) { + LOG_ERROR("Build update lidar cfg request failed, init host ip addr failed."); return false; } - const uint16_t lidar_point_port = kHAPPointDataPort; - memcpy(&(host_point_ip_info_val->host_port), &lidar_point_port, sizeof(lidar_point_port)); + uint16_t lidar_point_port = 0; + if (lidar_cfg.device_type == kLivoxLidarTypeIndustrialHAP) { + lidar_point_port = kHAPPointDataPort; + } else if (lidar_cfg.device_type == kLivoxLidarTypeMid360) { + lidar_point_port = kMid360LidarPointCloudPort; + } else if (lidar_cfg.device_type == kLivoxLidarTypePA) { + lidar_point_port = kPaLidarPointCloudPort; + } else { + LOG_ERROR("Build update lidar cfg request failed, unknown the dev_type:{}", lidar_cfg.device_type); + return false; + } + memcpy(&(host_point_ip_info_val->host_port), &lidar_cfg.host_net_info.point_data_port, sizeof(lidar_cfg.host_net_info.point_data_port)); memcpy(&(host_point_ip_info_val->lidar_port), &lidar_point_port, sizeof(lidar_point_port)); req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue); - + if (lidar_cfg.device_type == kLivoxLidarTypePA) { + return true; + } LivoxLidarKeyValueParam * imu_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len]; imu_kv->key = static_cast(kKeyLidarImuHostIPCfg); imu_kv->length = sizeof(uint8_t) * 8; HostIpInfoValue* host_imu_ip_info_val = (HostIpInfoValue*)&imu_kv->value; if (!InitHostIpAddr(lidar_cfg.host_net_info.imu_data_ip, host_imu_ip_info_val)) { + LOG_ERROR("Build update lidar cfg request failed, init imu host ip addr failed."); return false; } - const uint16_t lidar_imu_port = kHAPIMUPort; - memcpy(&(host_point_ip_info_val->host_port), &lidar_imu_port, sizeof(lidar_imu_port)); - memcpy(&(host_point_ip_info_val->lidar_port), &lidar_imu_port, sizeof(lidar_imu_port)); + uint16_t lidar_imu_port = 0; + if (lidar_cfg.device_type == kLivoxLidarTypeIndustrialHAP) { + lidar_imu_port = kHAPIMUPort; + } else if (lidar_cfg.device_type == kLivoxLidarTypeMid360) { + lidar_imu_port = kMid360LidarImuDataPort; + } else { + LOG_ERROR("Build update lidar cfg request failed, unknown the dev_type:{}", lidar_cfg.device_type); + return false; + } + + memcpy(&(host_imu_ip_info_val->host_port), &lidar_cfg.host_net_info.imu_data_port, sizeof(lidar_cfg.host_net_info.imu_data_port)); + memcpy(&(host_imu_ip_info_val->lidar_port), &lidar_imu_port, sizeof(lidar_imu_port)); req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue); + // LOG_ERROR("Build imu host ip:{}, host_port:{}, lidar_port:{}", lidar_cfg.host_net_info.imu_data_ip.c_str(), + // lidar_cfg.host_net_info.imu_data_port, lidar_imu_port); return true; } @@ -126,6 +203,7 @@ bool BuildRequest::BuildSetLidarIPInfoRequest(const LivoxLidarIpInfo& lidar_ip_c kv->length = sizeof(uint8_t) * 12; LivoxLidarIpInfoValue* lidar_ip_val = (LivoxLidarIpInfoValue*)&kv->value; if (!InitLidarIpinfoVal(lidar_ip_config, lidar_ip_val)) { + LOG_ERROR("Build set lidar ip info request failed, init lidar ip addr failed."); return false; } @@ -133,6 +211,29 @@ bool BuildRequest::BuildSetLidarIPInfoRequest(const LivoxLidarIpInfo& lidar_ip_c return true; } +bool BuildRequest::BuildSetHostStateInfoIPCfgRequest(const HostStateInfoIpInfo& host_state_info_ipcfg, + uint8_t* req_buf, uint16_t& req_len) { + uint16_t key_num = 1; + memcpy(&req_buf[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buf[req_len]; + kv->key = static_cast(kKeyStateInfoHostIPCfg); + kv->length = sizeof(uint8_t) * 8; + HostIpInfoValue* host_ip_info_val = (HostIpInfoValue*)&kv->value; + + if (!InitHostIpAddr(host_state_info_ipcfg.host_ip_addr, host_ip_info_val)) { + LOG_ERROR("Build set host point data ip info request failed, init host ip addr failed."); + return false; + } + + memcpy(&(host_ip_info_val->host_port), &host_state_info_ipcfg.host_state_info_port, sizeof(host_state_info_ipcfg.host_state_info_port)); + memcpy(&(host_ip_info_val->lidar_port), &host_state_info_ipcfg.lidar_state_info_port, sizeof(host_state_info_ipcfg.lidar_state_info_port)); + + req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue); + return true; +} + bool BuildRequest::BuildSetHostPointDataIPInfoRequest(const HostPointIPInfo& host_point_ip_cfg, uint8_t* req_buf, uint16_t& req_len) { uint16_t key_num = 1; memcpy(&req_buf[req_len], &key_num, sizeof(key_num)); @@ -144,6 +245,7 @@ bool BuildRequest::BuildSetHostPointDataIPInfoRequest(const HostPointIPInfo& hos HostIpInfoValue* host_ip_info_val = (HostIpInfoValue*)&kv->value; if (!InitHostIpAddr(host_point_ip_cfg.host_ip_addr, host_ip_info_val)) { + LOG_ERROR("Build set host point data ip info request failed, init host ip addr failed."); return false; } @@ -165,6 +267,7 @@ bool BuildRequest::BuildSetHostImuDataIPInfoRequest(const HostImuDataIPInfo& hos HostIpInfoValue* host_ip_info_val = (HostIpInfoValue*)&kv->value; if (!InitHostIpAddr(host_imu_ipcfg.host_ip_addr, host_ip_info_val)) { + LOG_ERROR("Build set host imu data ip info request failed, init host ip addr failed."); return false; } diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/build_request.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/build_request.h index ccdfa9a5..22ff2218 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/build_request.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/build_request.h @@ -48,17 +48,15 @@ class BuildRequest { public: static bool BuildUpdateViewLidarCfgRequest(const ViewLidarIpInfo& view_lidar_info, uint8_t* req_buf, uint16_t& req_len); static bool BuildUpdateLidarCfgRequest(const LivoxLidarCfg& lidar_cfg, uint8_t* req_buf, uint16_t& req_len); + static bool BuildUpdateMid360LidarCfgRequest(const LivoxLidarCfg& lidar_cfg, uint8_t* req_buf, uint16_t& req_len); static bool BuildSetLidarIPInfoRequest(const LivoxLidarIpInfo& ip_config, uint8_t* req_buf, uint16_t& req_len); + static bool BuildSetHostStateInfoIPCfgRequest(const HostStateInfoIpInfo& host_state_info_ipcfg, uint8_t* req_buf, uint16_t& req_len); static bool BuildSetHostPointDataIPInfoRequest(const HostPointIPInfo& lidar_ip_config, uint8_t* req_buf, uint16_t& req_len); static bool BuildSetHostImuDataIPInfoRequest(const HostImuDataIPInfo& host_imu_ipcfg, uint8_t* req_buf, uint16_t& req_len); static bool IpToU8(const std::string& src, const std::string& seq, std::vector& result); private: static bool InitLidarIpinfoVal(const LivoxLidarIpInfo& lidar_ip_config, LivoxLidarIpInfoValue* lidar_ipinfo_val_ptr); static bool InitHostIpAddr(const std::string& host_ip, HostIpInfoValue* host_ipinfo_val_ptr); - - - - }; } // namespace direct diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_handler.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_handler.h index c29883e6..0a852d30 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_handler.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_handler.h @@ -50,7 +50,7 @@ class CommandHandler { virtual bool Init(std::shared_ptr>& lidars_cfg_ptr, std::shared_ptr>& custom_lidars_cfg_ptr) = 0; - virtual void Handle(const uint32_t handle, const Command& command) = 0; + virtual void Handle(const uint32_t handle, uint16_t lidar_port, const Command& command) = 0; virtual void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) = 0; virtual void UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) = 0; virtual livox_status SendCommand(const Command& command) = 0; diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_impl.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_impl.cpp new file mode 100644 index 00000000..f29b7619 --- /dev/null +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_impl.cpp @@ -0,0 +1,693 @@ +// +// 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 "command_impl.h" + +#include "livox_lidar_def.h" +#include "general_command_handler.h" + +#include "base/logging.h" +#include "comm/protocol.h" +#include "comm/generate_seq.h" + +#include "build_request.h" + +namespace livox { +namespace lidar { + +livox_status CommandImpl::QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + std::set key_sets; + + if (!GeneralCommandHandler::GetInstance().GetQueryLidarInternalInfoKeys(handle, key_sets)) { + LOG_ERROR("Query livox lidar internal info failed."); + return kLivoxLidarStatusInvalidHandle; + } + + uint16_t key_num = key_sets.size(); + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + for (const auto &key : key_sets) { + LivoxLidarKeyValueParam* kList = (LivoxLidarKeyValueParam*)&req_buff[req_len]; + kList->key = static_cast(key); + req_len += sizeof(uint16_t); + } + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarGetInternalInfo, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + + +livox_status CommandImpl::SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyPclDataType); + kv->length = sizeof(uint8_t); + kv->value[0] = static_cast(data_type); + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyPatternMode); + kv->length = sizeof(uint8_t); + kv->value[0] = static_cast(scan_pattern); + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyDualEmitEnable); + kv->length = sizeof(uint8_t); + if (enable) { + kv->value[0] = 0x01; + } else { + kv->value[0] = 0x00; + } + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyPointSendEnable); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x00; + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyPointSendEnable); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x01; + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarIp(uint32_t handle, const LivoxLidarIpInfo* ip_config, + LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + if (!BuildRequest::BuildSetLidarIPInfoRequest(*ip_config, req_buff, req_len)) { + return -1; + } + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarStateInfoHostIPCfg(uint32_t handle, const HostStateInfoIpInfo& host_state_info_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + if (!BuildRequest::BuildSetHostStateInfoIPCfgRequest(host_state_info_ipcfg, req_buff, req_len)) { + return -1; + } + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarPointDataHostIPCfg(uint32_t handle, const HostPointIPInfo& host_point_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + if (!BuildRequest::BuildSetHostPointDataIPInfoRequest(host_point_ipcfg, req_buff, req_len)) { + return -1; + } + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarImuDataHostIPCfg(uint32_t handle, const HostImuDataIPInfo& host_imu_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + if (!BuildRequest::BuildSetHostImuDataIPInfoRequest(host_imu_ipcfg, req_buff, req_len)) { + return -1; + } + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + + +livox_status CommandImpl::SetLivoxLidarInstallAttitude(uint32_t handle, const LivoxLidarInstallAttitude& install_attitude, + LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyInstallAttitude); + kv->length = sizeof(LivoxLidarInstallAttitude); + + LivoxLidarInstallAttitude* install_attitude_val = (LivoxLidarInstallAttitude*)&kv->value; + memcpy(install_attitude_val, &install_attitude, sizeof(LivoxLidarInstallAttitude)); + req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(LivoxLidarInstallAttitude); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarRoiCfg0(uint32_t handle, const RoiCfg& roi_cfg0, LivoxLidarAsyncControlCallback cb, + void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyRoiCfg0); + kv->length = sizeof(RoiCfg); + + RoiCfg* roi_cfg = (RoiCfg*)&kv->value; + memcpy(roi_cfg, &roi_cfg0, sizeof(RoiCfg)); + req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(RoiCfg); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); + +} + +livox_status CommandImpl::SetLivoxLidarRoiCfg1(uint32_t handle, const RoiCfg& roi_cfg1, LivoxLidarAsyncControlCallback cb, + void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyRoiCfg1); + kv->length = sizeof(RoiCfg); + + RoiCfg* roi_cfg = (RoiCfg*)&kv->value; + memcpy(roi_cfg, &roi_cfg1, sizeof(RoiCfg)); + req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(RoiCfg); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::EnableLivoxLidarRoi(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyRoiEn); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x01; + + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::DisableLivoxLidarRoi(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyRoiEn); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x00; + + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); + +} + +livox_status CommandImpl::SetLivoxLidarDetectMode(uint32_t handle, LivoxLidarDetectMode mode, + LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyDetectMode); + kv->length = sizeof(uint8_t); + kv->value[0] = static_cast(mode); + + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarFuncIOCfg(uint32_t handle, const FuncIOCfg& func_io_cfg, + LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyFuncIOCfg); + kv->length = sizeof(FuncIOCfg); + + FuncIOCfg* func_io_cfg_val = (FuncIOCfg*)&kv->value; + memcpy(func_io_cfg_val, &func_io_cfg, sizeof(FuncIOCfg)); + req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(FuncIOCfg); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + + +livox_status CommandImpl::SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + //blind spot + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyBlindSpotSet); + kv->length = sizeof(uint32_t); + uint32_t* blind_spot_set = reinterpret_cast(&kv->value[0]); + *blind_spot_set = blind_spot; + req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint32_t); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyWorkMode); + kv->length = sizeof(uint8_t); + uint8_t* val_work_mode = reinterpret_cast(&kv->value[0]); + *val_work_mode = work_mode; + req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint8_t); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + + +livox_status CommandImpl::EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + //glass heat + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyGlassHeat); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x01; //enable glass heat + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + //glass heat + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyGlassHeat); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x00; //disable glass heat + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + //glass heat + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyGlassHeat); + kv->length = sizeof(uint8_t); + kv->value[0] = static_cast(glass_heat); + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + + +livox_status CommandImpl::EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyImuDataEn); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x01; + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyImuDataEn); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x00; + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + //glass heat + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyFusaEn); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x01; //enable glass heat + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + //glass heat + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyFusaEn); + kv->length = sizeof(uint8_t); + kv->value[0] = 0x00; //disable glass heat + req_len += sizeof(LivoxLidarKeyValueParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::SetLivoxLidarLogParam(uint32_t handle, const LivoxLidarLogParam& log_param, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyLogParamSet); + kv->length = sizeof(LivoxLidarLogParam); + LivoxLidarLogParam* log_param_val = (LivoxLidarLogParam*)&kv->value; + memcpy(log_param_val, &log_param, sizeof(LivoxLidarLogParam)); + + req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(LivoxLidarLogParam); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + + +livox_status CommandImpl::LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) { + return GeneralCommandHandler::GetInstance().LivoxLidarRequestReset(handle, cb, client_data); +} + +// Upgrade +livox_status CommandImpl::LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length, + LivoxLidarStartUpgradeCallback cb, void* client_data) { + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDGeneralRequestUpgrade, + data, + length, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, + LivoxLidarXferFirmwareCallback cb, void* client_data) { + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDGeneralXferFirmware, + data, + length, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, + LivoxLidarCompleteXferFirmwareCallback cb, void* client_data) { + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDGeneralCompleteXferFirmware, + data, + length, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data, + uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data) { + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDGeneralRequestUpgradeProgress, + data, + length, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::LivoxLidarRequestFirmwareInfo(uint32_t handle, + LivoxLidarRequestFirmwareInfoCallback cb, void* client_data) { + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDGeneralRequestFirmwareInfo, + nullptr, + 0, + MakeCommandCallback(cb, client_data)); +} + +livox_status CommandImpl::LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, + void* client_data) { + LivoxLidarRebootRequest reboot_request; + reboot_request.timeout = 100; + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarRebootDevice, (uint8_t *)&reboot_request, + sizeof(reboot_request), MakeCommandCallback(cb, + client_data)); +} + +} // namespace livox +} // namespace lidar + diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_impl.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_impl.h new file mode 100644 index 00000000..77bc7559 --- /dev/null +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/command_impl.h @@ -0,0 +1,127 @@ +// +// 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. +// + +#ifndef COMMAND_IMPL_H_ +#define COMMAND_IMPL_H_ + +#include +#include +#include + +#include "base/command_callback.h" +#include "base/io_thread.h" + +#include "comm/protocol.h" +#include "comm/define.h" + +#include "livox_lidar_api.h" + +#include "livox_lidar_def.h" +#include "device_manager.h" +#include "command_handler.h" + +namespace livox { +namespace lidar { + +class CommandImpl { + public: + static livox_status QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data); + + static livox_status SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarIp(uint32_t handle, const LivoxLidarIpInfo* ip_config, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status SetLivoxLidarStateInfoHostIPCfg(uint32_t handle, const HostStateInfoIpInfo& host_state_info_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status SetLivoxLidarPointDataHostIPCfg(uint32_t handle, const HostPointIPInfo& host_point_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarImuDataHostIPCfg(uint32_t handle, const HostImuDataIPInfo& host_imu_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarInstallAttitude(uint32_t handle, const LivoxLidarInstallAttitude& install_attitude, + LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarRoiCfg0(uint32_t handle, const RoiCfg& roi_cfg0, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status SetLivoxLidarRoiCfg1(uint32_t handle, const RoiCfg& roi_cfg1, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status EnableLivoxLidarRoi(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status DisableLivoxLidarRoi(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarDetectMode(uint32_t handle, LivoxLidarDetectMode mode, + LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarFuncIOCfg(uint32_t handle, const FuncIOCfg& func_io_cfg, + LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + static livox_status DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status SetLivoxLidarLogParam(uint32_t handle, const LivoxLidarLogParam& log_param, LivoxLidarAsyncControlCallback cb, void* client_data); + + static livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data); + + /*******Upgrade Module***********/ + static livox_status LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, void* client_data); + + /** + * Upgrade related command + */ + static livox_status LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length, + LivoxLidarStartUpgradeCallback cb, void* client_data); + + static livox_status LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, + LivoxLidarXferFirmwareCallback cb, void* client_data); + + static livox_status LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data, + uint16_t length, LivoxLidarCompleteXferFirmwareCallback cb, void* client_data); + + static livox_status LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data, + uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data); + + + static livox_status LivoxLidarRequestFirmwareInfo(uint32_t handle, + LivoxLidarRequestFirmwareInfoCallback cb, void* client_data); +}; + +} // namespace livox +} // namespace lidar + +#endif // COMMAND_IMPL_H_ diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/general_command_handler.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/general_command_handler.cpp index 304e3ac1..a70fe24e 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/general_command_handler.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/general_command_handler.cpp @@ -27,6 +27,8 @@ #include "livox_lidar_def.h" #include "command_handler/command_handler.h" #include "command_handler/hap_command_handler.h" +#include "command_handler/mid360_command_handler.h" +#include "command_handler/pa_command_handler.h" #include "base/logging.h" #include "comm/protocol.h" @@ -50,13 +52,14 @@ bool GeneralCommandHandler::Init(const std::string& host_ip, const bool is_view, return true; } - - bool GeneralCommandHandler::Init(std::shared_ptr>& lidars_cfg_ptr, std::shared_ptr>& custom_lidars_cfg_ptr, DeviceManager* device_manager) { + is_view_ = false; comm_port_.reset(new CommPort()); + device_manager_ = device_manager; for (auto it = lidars_cfg_ptr->begin(); it != lidars_cfg_ptr->end(); ++it) { const uint8_t dev_type = it->device_type; + std::lock_guard lock(command_handle_mutex_); if (lidars_command_handler_.find(dev_type) == lidars_command_handler_.end()) { if (dev_type == kLivoxLidarTypeIndustrialHAP) { std::shared_ptr hap_command_handler_ptr(new HapCommandHandler(device_manager)); @@ -65,26 +68,50 @@ bool GeneralCommandHandler::Init(std::shared_ptr>& li LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type); return false; } + } else if (dev_type == kLivoxLidarTypeMid360) { + std::shared_ptr mid360_command_handler_ptr(new Mid360CommandHandler(device_manager)); + lidars_command_handler_[dev_type] = mid360_command_handler_ptr; + if (!(lidars_command_handler_[dev_type]->Init(lidars_cfg_ptr, custom_lidars_cfg_ptr))) { + LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type); + return false; + } + } else if (dev_type == kLivoxLidarTypePA) { + std::shared_ptr pa_command_handler_ptr(new PaCommandHandler(device_manager)); + lidars_command_handler_[dev_type] = pa_command_handler_ptr; + if (!(lidars_command_handler_[dev_type]->Init(lidars_cfg_ptr, custom_lidars_cfg_ptr))) { + LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type); + return false; + } } } } for (auto it = custom_lidars_cfg_ptr->begin(); it != custom_lidars_cfg_ptr->end(); ++it) { const uint8_t dev_type = it->device_type; + std::lock_guard lock(command_handle_mutex_); if (lidars_command_handler_.find(dev_type) == lidars_command_handler_.end()) { if (dev_type == kLivoxLidarTypeIndustrialHAP) { - lidars_command_handler_[dev_type].reset(new HapCommandHandler(device_manager)); + lidars_command_handler_[dev_type].reset(new HapCommandHandler(device_manager_)); if (!(lidars_command_handler_[dev_type]->Init(lidars_cfg_ptr, custom_lidars_cfg_ptr))) { LOG_ERROR("General command handler init failed, the custom lidar of type:{} command init failed.", dev_type); return false; } + } else if (dev_type == kLivoxLidarTypeMid360) { + std::shared_ptr mid360_command_handler_ptr(new Mid360CommandHandler(device_manager)); + lidars_command_handler_[dev_type] = mid360_command_handler_ptr; + if (!(lidars_command_handler_[dev_type]->Init(lidars_cfg_ptr, custom_lidars_cfg_ptr))) { + LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type); + return false; + } } } } - device_manager_ = device_manager; + return true; } +void GeneralCommandHandler::CreateCommandHandler(const uint8_t dev_type) { +} void GeneralCommandHandler::Destory() { device_manager_ = nullptr; @@ -99,7 +126,10 @@ void GeneralCommandHandler::Destory() { devices_.clear(); } - lidars_command_handler_.clear(); + { + std::lock_guard lock(command_handle_mutex_); + lidars_command_handler_.clear(); + } { std::mutex commands_mutex_; @@ -134,7 +164,6 @@ void GeneralCommandHandler::Handler(uint32_t handle, uint16_t lidar_port, uint8_ if (packet.cmd_type == kCommandTypeCmd) { return; } - HandleDetectionData(handle, lidar_port, packet); } else if (packet.cmd_id == kCommandIDLidarGetInternalInfo) { uint32_t seq = packet.seq_num; @@ -153,6 +182,15 @@ void GeneralCommandHandler::Handler(uint32_t handle, uint16_t lidar_port, uint8_ if (command.cb) { (*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data); } + } else if (packet.cmd_id == kCommandIDLidarPushMsg) { + std::shared_ptr cmd_handler = GetLidarCommandHandler(handle); + if (cmd_handler == nullptr) { + LOG_ERROR("Handler general command failed, get push msg command handler faield."); + return; + } + Command command; + command.packet = packet; + cmd_handler->Handle(handle, lidar_port, command); } } @@ -168,7 +206,6 @@ void GeneralCommandHandler::AddCommand(const Command& command) { cmd.packet.data = NULL; cmd.packet.data_len = 0; } - } void GeneralCommandHandler::Handler(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_port, @@ -189,13 +226,10 @@ void GeneralCommandHandler::Handler(const uint8_t dev_type, const uint32_t handl LOG_INFO("GeneralCommandHandler::Handler Recv detection buf_size:{}", buf_size); return; } - HandleDetectionData(handle, lidar_port, packet); return; } - - LOG_INFO("GeneralCommandHandler::Handler enter hap command handle, cmd_type:{}", packet.cmd_type); std::shared_ptr cmd_handler = GetLidarCommandHandler(dev_type); if (cmd_handler == nullptr) { LOG_ERROR("GeneralCommandHandler::Handler get cmd handler failed"); @@ -215,7 +249,7 @@ void GeneralCommandHandler::Handler(const uint8_t dev_type, const uint32_t handl command.packet = packet; command.handle = handle; } - cmd_handler->Handle(handle, command); + cmd_handler->Handle(handle, lidar_port, command); } bool GeneralCommandHandler::VerifyNetSegment(const DetectionData* detection_data) { @@ -252,6 +286,8 @@ void GeneralCommandHandler::CreateCommandHandle(const uint8_t dev_type) { if (!is_view_) { return; } + + std::lock_guard lock(command_handle_mutex_); if (lidars_command_handler_.find(dev_type) == lidars_command_handler_.end()) { if (dev_type == kLivoxLidarTypeIndustrialHAP) { std::shared_ptr hap_command_handler_ptr(new HapCommandHandler(device_manager_)); @@ -259,6 +295,18 @@ void GeneralCommandHandler::CreateCommandHandle(const uint8_t dev_type) { if (!(lidars_command_handler_[dev_type]->Init(is_view_))) { LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type); } + } else if (dev_type == kLivoxLidarTypeMid360) { + std::shared_ptr mid360_command_handler_ptr(new Mid360CommandHandler(device_manager_)); + lidars_command_handler_[dev_type] = mid360_command_handler_ptr; + if (!(lidars_command_handler_[dev_type]->Init(is_view_))) { + LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type); + } + } else if (dev_type == kLivoxLidarTypePA) { + std::shared_ptr pa_command_handler_ptr(new PaCommandHandler(device_manager_)); + lidars_command_handler_[dev_type] = pa_command_handler_ptr; + if (!(lidars_command_handler_[dev_type]->Init(is_view_))) { + LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type); + } } } } @@ -314,7 +362,6 @@ void GeneralCommandHandler::HandleDetectionData(uint32_t handle, uint16_t lidar_ device_info.is_update_cfg.store(false); //devices_[handle] = std::move(device_info); - LOG_INFO("Handle detection data, handle:{}, dev_type:{}, sn:{}, lidar_ip:{}, cmd_port:{}", handle, detection_data->dev_type, detection_data->sn, lidar_ip.c_str(), detection_data->cmd_port); { @@ -326,7 +373,6 @@ void GeneralCommandHandler::HandleDetectionData(uint32_t handle, uint16_t lidar_ } } else { device_dev_type_[handle] = detection_data->dev_type; - LOG_INFO("Handle detection data, set buff handle:{}, dev_type:{}", handle, detection_data->dev_type); } } @@ -378,570 +424,137 @@ void GeneralCommandHandler::LivoxLidarInfoChange(const uint32_t handle) { } } -std::shared_ptr GeneralCommandHandler::GetLidarCommandHandler(const uint8_t dev_type) { - if (lidars_command_handler_.find(dev_type) != lidars_command_handler_.end()) { - return lidars_command_handler_[dev_type]; - } - return nullptr; -} - -livox_status GeneralCommandHandler::QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - std::set key_sets { - kKeyPclDataType, - kKeyPatternMode, - kKeyDualEmitEnable, - kKeyPointSendEnable, - kKeyLidarIPCfg, - kKeyLidarPointDataHostIPCfg, - kKeyLidarImuHostIPCfg, - kKeyInstallAttitude, - kKeyBlindSpotSet, - kKeyWorkMode, - kKeyGlassHeat, - kKeyImuDataEn, - kKeyFusaEn, - kKeySn, - kKeyProductInfo, - kKeyVersionApp, - kKeyVersionLoader, - kKeyVersionHardware, - kKeyMac, - kKeyCurWorkState, - kKeyStatusCode, - kKeyLidarDiagStatus, - kKeyLidarFlashStatus - }; - - uint16_t key_num = key_sets.size(); - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - for (const auto &key : key_sets) { - LivoxLidarKeyValueParam* kList = (LivoxLidarKeyValueParam*)&req_buff[req_len]; - kList->key = static_cast(key); - req_len += sizeof(uint16_t); - } - - return SendCommand(handle, - kCommandIDLidarGetInternalInfo, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - - -livox_status GeneralCommandHandler::SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyPclDataType); - kv->length = sizeof(uint8_t); - kv->value[0] = static_cast(data_type); - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - - -// livox_status GeneralCommandHandler::EnableLivoxLidarHighResolutionPointType(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { -// uint8_t req_buff[kMaxCommandBufferSize] = {0}; -// uint16_t req_len = 0; - -// uint16_t key_num = 1; -// memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); -// req_len = sizeof(key_num) + sizeof(uint16_t); - -// LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; -// kv->key = static_cast(kKeyPclDataType); -// kv->length = sizeof(uint8_t); -// kv->value[0] = static_cast(kLivoxLidarCartesianCoordinateHighData); //32bit point type -// req_len += sizeof(LivoxLidarKeyValueParam); - -// return SendCommand(handle, -// kCommandIDLidarWorkModeControl, -// req_buff, -// req_len, -// MakeCommandCallback(cb, client_data)); -// } - -// livox_status GeneralCommandHandler::DisableLivoxLidarHighResolutionPointType(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { -// uint8_t req_buff[kMaxCommandBufferSize] = {0}; -// uint16_t req_len = 0; - -// uint16_t key_num = 1; -// memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); -// req_len = sizeof(key_num) + sizeof(uint16_t); - -// LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; -// kv->key = static_cast(kKeyPclDataType); -// kv->length = sizeof(uint8_t); -// kv->value[0] = static_cast(kLivoxLidarCartesianCoordinateLowData); //16bit point type -// req_len += sizeof(LivoxLidarKeyValueParam); - -// return SendCommand(handle, -// kCommandIDLidarWorkModeControl, -// req_buff, -// req_len, -// MakeCommandCallback(cb, client_data)); -// } - -livox_status GeneralCommandHandler::SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyPatternMode); - kv->length = sizeof(uint8_t); - kv->value[0] = static_cast(scan_pattern); - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyDualEmitEnable); - kv->length = sizeof(uint8_t); - if (enable) { - kv->value[0] = 0x01; - } else { - kv->value[0] = 0x00; +void GeneralCommandHandler::PushLivoxLidarInfo(const uint32_t handle, const std::string& info) { + std::lock_guard lock(dev_type_mutex_); + if (device_dev_type_.find(handle) != device_dev_type_.end()) { + uint8_t dev_type = device_dev_type_[handle]; + + if (livox_lidar_info_cb_) { + livox_lidar_info_cb_(handle, dev_type, info.c_str(), livox_lidar_info_client_data_); + } } - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyPointSendEnable); - kv->length = sizeof(uint8_t); - kv->value[0] = 0x00; - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyPointSendEnable); - kv->length = sizeof(uint8_t); - kv->value[0] = 0x01; - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); } -livox_status GeneralCommandHandler::SetLivoxLidarIp(uint32_t handle, const LivoxLidarIpInfo* ip_config, - LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - if (!BuildRequest::BuildSetLidarIPInfoRequest(*ip_config, req_buff, req_len)) { - return -1; +std::shared_ptr GeneralCommandHandler::GetLidarCommandHandler(const uint32_t handle) { + std::lock_guard lock(dev_type_mutex_); + if (device_dev_type_.find(handle) != device_dev_type_.end()) { + uint8_t dev_type = device_dev_type_[handle]; + return GetLidarCommandHandler(dev_type); } - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); + LOG_ERROR("Get command handler failed, get dev type failed, the handle:{}", handle); + return nullptr; } -livox_status GeneralCommandHandler::SetLivoxLidarPointDataHostIPCfg(uint32_t handle, const HostPointIPInfo& host_point_ipcfg, - LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - if (!BuildRequest::BuildSetHostPointDataIPInfoRequest(host_point_ipcfg, req_buff, req_len)) { - return -1; +std::shared_ptr GeneralCommandHandler::GetLidarCommandHandler(const uint8_t dev_type) { + std::lock_guard lock(command_handle_mutex_); + if (lidars_command_handler_.find(dev_type) != lidars_command_handler_.end()) { + return lidars_command_handler_[dev_type]; } - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); + LOG_ERROR("Get lidar command handler failed, the dev type:{}", dev_type); + return nullptr; } -livox_status GeneralCommandHandler::SetLivoxLidarImuDataHostIPCfg(uint32_t handle, const HostImuDataIPInfo& host_imu_ipcfg, - LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - if (!BuildRequest::BuildSetHostImuDataIPInfoRequest(host_imu_ipcfg, req_buff, req_len)) { - return -1; +bool GeneralCommandHandler::GetQueryLidarInternalInfoKeys(const uint32_t handle, std::set& key_sets) { + std::lock_guard lock(dev_type_mutex_); + if (device_dev_type_.find(handle) != device_dev_type_.end()) { + uint8_t dev_type = device_dev_type_[handle]; + if (dev_type == kLivoxLidarTypeIndustrialHAP) { + std::set tmp_key_sets { + kKeyPclDataType, + kKeyPatternMode, + kKeyLidarIPCfg, + kKeyLidarPointDataHostIPCfg, + kKeyLidarImuHostIPCfg, + kKeyInstallAttitude, + kKeyWorkMode, + kKeyImuDataEn, + kKeySn, + kKeyProductInfo, + kKeyVersionApp, + kKeyVersionLoader, + kKeyVersionHardware, + kKeyMac, + kKeyCurWorkState + }; + key_sets.swap(tmp_key_sets); + return true; + } else if (dev_type == kLivoxLidarTypeMid360) { + std::set tmp_key_sets { + kKeyPclDataType, + kKeyPatternMode, + kKeyLidarIPCfg, + kKeyStateInfoHostIPCfg, + kKeyLidarPointDataHostIPCfg, + kKeyLidarImuHostIPCfg, + kKeyInstallAttitude, + kKeyRoiCfg0, + kKeyRoiCfg1, + kKeyRoiEn, + kKeyWorkMode, + kKeyImuDataEn, + kKeySn, + kKeyProductInfo, + kKeyVersionApp, + kKeyVersionLoader, + kKeyVersionHardware, + kKeyMac, + kKeyCurWorkState, + kKeyCoreTemp, + kKeyPowerUpCnt, + kKeyLocalTimeNow, + kKeyLastSyncTime, + kKeyTimeOffset, + kKeyTimeSyncType, + kKeyLidarDiagStatus, + kKeyFwType, + kKeyHmsCode + }; + key_sets.swap(tmp_key_sets); + return true; + } else if (dev_type == kLivoxLidarTypePA) { + std::set tmp_key_sets { + kKeyLidarIPCfg, + kKeyLidarPointDataHostIPCfg, + kKeyWorkMode, + kKeyGlassHeat, + kKeySn, + kKeyProductInfo, + kKeyVersionApp, + kKeyVersionLoader, + kKeyVersionHardware, + kKeyMac, + kKeyCurWorkState, + kKeyTimeSyncType, + kKeyStatusCode, + kKeyLidarDiagStatus, + kKeyFwType + }; + key_sets.swap(tmp_key_sets); + return true; + } } - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - - -livox_status GeneralCommandHandler::SetLivoxLidarInstallAttitude(uint32_t handle, const LivoxLidarInstallAttitude& install_attitude, - LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyInstallAttitude); - kv->length = sizeof(LivoxLidarInstallAttitude); - - LivoxLidarInstallAttitude* install_attitude_val = (LivoxLidarInstallAttitude*)&kv->value; - memcpy(install_attitude_val, &install_attitude, sizeof(LivoxLidarInstallAttitude)); - req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(LivoxLidarInstallAttitude); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); + return false; } -livox_status GeneralCommandHandler::SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - //blind spot - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyBlindSpotSet); - kv->length = sizeof(uint32_t); - uint32_t* blind_spot_set = reinterpret_cast(&kv->value[0]); - *blind_spot_set = blind_spot; - req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint32_t); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyWorkMode); - kv->length = sizeof(uint8_t); - uint8_t* val_work_mode = reinterpret_cast(&kv->value[0]); - *val_work_mode = work_mode; - req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint8_t); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - - -livox_status GeneralCommandHandler::EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - //glass heat - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyGlassHeat); - kv->length = sizeof(uint8_t); - kv->value[0] = 0x01; //enable glass heat - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - //glass heat - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyGlassHeat); - kv->length = sizeof(uint8_t); - kv->value[0] = 0x00; //disable glass heat - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - //glass heat - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyGlassHeat); - kv->length = sizeof(uint8_t); - kv->value[0] = static_cast(glass_heat); - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - - -livox_status GeneralCommandHandler::EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyImuDataEn); - kv->length = sizeof(uint8_t); - kv->value[0] = 0x01; - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyImuDataEn); - kv->length = sizeof(uint8_t); - kv->value[0] = 0x00; - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - //glass heat - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyFusaEn); - kv->length = sizeof(uint8_t); - kv->value[0] = 0x01; //enable glass heat - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - //glass heat - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyFusaEn); - kv->length = sizeof(uint8_t); - kv->value[0] = 0x00; //disable glass heat - req_len += sizeof(LivoxLidarKeyValueParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::SetLivoxLidarLogParam(uint32_t handle, const LivoxLidarLogParam& log_param, LivoxLidarAsyncControlCallback cb, void* client_data) { - uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = 0; - - uint16_t key_num = 1; - memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); - req_len = sizeof(key_num) + sizeof(uint16_t); - - LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; - kv->key = static_cast(kKeyLogParamSet); - kv->length = sizeof(LivoxLidarLogParam); - LivoxLidarLogParam* log_param_val = (LivoxLidarLogParam*)&kv->value; - memcpy(log_param_val, &log_param, sizeof(LivoxLidarLogParam)); - - req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(LivoxLidarLogParam); - - return SendCommand(handle, - kCommandIDLidarWorkModeControl, - req_buff, - req_len, - MakeCommandCallback(cb, client_data)); -} - - livox_status GeneralCommandHandler::LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) { LivoxLidarResetRequest reset_request; - return SendCommand(handle, kCommandIDLidarRebootDevice, (uint8_t*)&reset_request, sizeof(LivoxLidarResetRequest), - MakeCommandCallback(cb, client_data)); -} - -// Upgrade -livox_status GeneralCommandHandler::LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length, - LivoxLidarStartUpgradeCallback cb, void* client_data) { - return SendCommand(handle, - kCommandIDGeneralRequestUpgrade, - data, - length, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, - LivoxLidarXferFirmwareCallback cb, void* client_data) { - return SendCommand(handle, - kCommandIDGeneralXferFirmware, - data, - length, - MakeCommandCallback(cb, client_data)); -} - -livox_status GeneralCommandHandler::LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, - LivoxLidarCompleteXferFirmwareCallback cb, void* client_data) { - return SendCommand(handle, - kCommandIDGeneralCompleteXferFirmware, - data, - length, - MakeCommandCallback(cb, client_data)); -} + std::string sn; + if (devices_.find(handle) != devices_.end()) { + sn = devices_[handle].sn; + } else { + return kLivoxLidarStatusChannelNotExist; + } -livox_status GeneralCommandHandler::LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data, - uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data) { - return SendCommand(handle, - kCommandIDGeneralRequestUpgradeProgress, - data, - length, - MakeCommandCallback(cb, client_data)); -} + if (sn.size() > 16) { + LOG_ERROR("Request reset failed, the sn size too long, the sn:", sn.c_str()); + return kLivoxLidarStatusChannelNotExist; + } -livox_status GeneralCommandHandler::LivoxLidarRequestFirmwareInfo(uint32_t handle, - LivoxLidarRequestFirmwareInfoCallback cb, void* client_data) { - return SendCommand(handle, - kCommandIDGeneralRequestFirmwareInfo, - nullptr, - 0, - MakeCommandCallback(cb, client_data)); + memcpy(reset_request.data, sn.c_str(), sn.size()); + return SendCommand(handle, kCommandIDLidarResetDevice, (uint8_t*)&reset_request, sizeof(LivoxLidarResetRequest), + MakeCommandCallback(cb, client_data)); } -livox_status GeneralCommandHandler::LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, - void* client_data) { - LivoxLidarRebootRequest reboot_request; - reboot_request.timeout = 100; - return SendCommand(handle, - kCommandIDLidarRebootDevice, (uint8_t *)&reboot_request, - sizeof(reboot_request), MakeCommandCallback(cb, - client_data)); -} livox_status GeneralCommandHandler::SendCommand(uint32_t handle, uint16_t command_id, @@ -954,21 +567,18 @@ livox_status GeneralCommandHandler::SendCommand(uint32_t handle, uint16_t seq = GenerateSeq::GetSeq(); Command command(seq, command_id, kCommandTypeCmd, kHostSend, data, length, handle, lidar_ip, cb); - LOG_INFO("Send command, the seq:{}, command_id:{}, lidar_ip:{}", seq, command_id, lidar_ip.c_str()); - - if (device_dev_type_.find(handle) != device_dev_type_.end()) { - uint8_t dev_type = device_dev_type_[handle]; - std::shared_ptr cmd_handler = GetLidarCommandHandler(dev_type); - if (cmd_handler != nullptr) { - cmd_handler->SendCommand(command); - } + std::shared_ptr cmd_handler = GetLidarCommandHandler(handle); + if (cmd_handler == nullptr) { + LOG_ERROR("Send command failed, get cmd handler failed, the handle:{}, command_id:{}.", handle, command_id); + return kLivoxLidarStatusSendFailed; } + cmd_handler->SendCommand(command); + { std::lock_guard lock(commands_mutex_); commands_[command.packet.seq_num] = std::make_pair(command, std::chrono::steady_clock::now() + std::chrono::milliseconds(command.time_out)); Command &cmd = commands_[command.packet.seq_num].first; - LOG_INFO("Command_ Command_id:{}, seq:{}", command_id, command.packet.seq_num); if (cmd.packet.data != NULL) { cmd.packet.data = NULL; @@ -982,7 +592,6 @@ livox_status GeneralCommandHandler::SendCommand(uint32_t handle, void GeneralCommandHandler::CommandsHandle(TimePoint now) { std::list timeout_commands; - { std::lock_guard lock(commands_mutex_); std::map >::iterator ite = commands_.begin(); diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/general_command_handler.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/general_command_handler.h index f9da98c4..abf53ce0 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/general_command_handler.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/general_command_handler.h @@ -35,7 +35,7 @@ #include "comm/protocol.h" #include "comm/define.h" -#include "livox_lidar_sdk.h" +#include "livox_lidar_api.h" #include "livox_lidar_def.h" #include "device_manager.h" @@ -75,6 +75,9 @@ class GeneralCommandHandler : public noncopyable { void Handler(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_port, uint8_t *buf, uint32_t buf_size); + livox_status SendCommand(uint32_t handle, uint16_t command_id, uint8_t *data, + uint16_t length, const std::shared_ptr &cb); + void CommandsHandle(TimePoint now); void AddCommand(const Command& command); @@ -82,87 +85,25 @@ class GeneralCommandHandler : public noncopyable { livox_lidar_info_change_cb_ = cb; livox_lidar_info_change_client_data_ = client_data; } + + void SetLivoxLidarInfoCallback(LivoxLidarInfoCallback cb, void* client_data) { + livox_lidar_info_cb_ = cb; + livox_lidar_info_client_data_ = client_data; + } + void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info); void UpdateLidarCfg(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_cmd_port); void LivoxLidarInfoChange(const uint32_t handle); + void PushLivoxLidarInfo(const uint32_t handle, const std::string& info); + bool GetQueryLidarInternalInfoKeys(const uint32_t handle, std::set& key_sets); + livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data); private: + void CreateCommandHandler(const uint8_t dev_type); bool VerifyNetSegment(const DetectionData* detection_data); void CreateCommandHandle(const uint8_t dev_type); - public: - // livox_status GetFrameRate(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data); - // livox_status QueryLidarStateInfo(uint32_t handle, OnQueryLidarStateInfoCallback cb, void* client_data); - //livox_status QueryLivoxLidarIpInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data); - livox_status QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data); - - livox_status SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data); - // livox_status EnableLivoxLidarHighResolutionPointType(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - // livox_status DisableLivoxLidarHighResolutionPointType(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - livox_status DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarIp(uint32_t handle, const LivoxLidarIpInfo* ip_config, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarPointDataHostIPCfg(uint32_t handle, const HostPointIPInfo& host_point_ipcfg, - LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarImuDataHostIPCfg(uint32_t handle, const HostImuDataIPInfo& host_imu_ipcfg, - LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarInstallAttitude(uint32_t handle, const LivoxLidarInstallAttitude& install_attitude, - LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data); - - // livox_status SetLivoxLidarFrameRate(uint32_t handle, LivoxLidarPointFrameRate rate, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - livox_status DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - livox_status SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - livox_status DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - livox_status DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status SetLivoxLidarLogParam(uint32_t handle, const LivoxLidarLogParam& log_param, LivoxLidarAsyncControlCallback cb, void* client_data); - - livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data); - - /*******Upgrade Module***********/ - livox_status LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, void* client_data); - - /** - * Upgrade related command - */ - livox_status LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length, - LivoxLidarStartUpgradeCallback cb, void* client_data); - - livox_status LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, - LivoxLidarXferFirmwareCallback cb, void* client_data); - - livox_status LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data, - uint16_t length, LivoxLidarCompleteXferFirmwareCallback cb, void* client_data); - - livox_status LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data, - uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data); - - - livox_status LivoxLidarRequestFirmwareInfo(uint32_t handle, - LivoxLidarRequestFirmwareInfoCallback cb, void* client_data); - - private: std::shared_ptr GetLidarCommandHandler(const uint8_t dev_type); + std::shared_ptr GetLidarCommandHandler(const uint32_t handle); void HandleDetectionData(uint32_t handle, uint16_t lidar_port, const CommPacket& packet); - livox_status SendCommand(uint32_t handle, uint16_t command_id, uint8_t *data, - uint16_t length, const std::shared_ptr &cb); private: DeviceManager* device_manager_; std::unique_ptr comm_port_; @@ -173,6 +114,7 @@ class GeneralCommandHandler : public noncopyable { std::mutex devices_mutex_; std::map devices_; + std::mutex command_handle_mutex_; std::map> lidars_command_handler_; std::mutex commands_mutex_; @@ -181,6 +123,9 @@ class GeneralCommandHandler : public noncopyable { LivoxLidarInfoChangeCallback livox_lidar_info_change_cb_; void* livox_lidar_info_change_client_data_; + LivoxLidarInfoCallback livox_lidar_info_cb_; + void* livox_lidar_info_client_data_; + std::string host_ip_; bool is_view_; diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/hap_command_handler.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/hap_command_handler.cpp index b50c1472..203e8ea5 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/hap_command_handler.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/hap_command_handler.cpp @@ -65,7 +65,7 @@ bool HapCommandHandler::Init(std::shared_ptr>& lidars return true; } -void HapCommandHandler::Handle(const uint32_t handle, const Command& command) { +void HapCommandHandler::Handle(const uint32_t handle, uint16_t lidar_port, const Command& command) { if (command.packet.cmd_type == kCommandTypeAck) { LOG_INFO(" Receive Ack: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num); OnCommandAck(handle, command); @@ -165,12 +165,9 @@ void HapCommandHandler::SetGeneralLidar(const uint32_t handle, const uint16_t li uint16_t seq = GenerateSeq::GetSeq(); Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, handle, lidar_ip, MakeCommandCallback(HapCommandHandler::UpdateLidarCallback, this)); - LOG_INFO("HapCommandHandler::SetGeneralLidar seq:{}, lidar_ip:{}", seq, lidar_ip.c_str()); SendCommand(command, lidar_cmd_port); } - - void HapCommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data) { if (status != kLivoxLidarStatusSuccess) { @@ -183,7 +180,6 @@ void HapCommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle return; } - LOG_INFO("HapCommandHandler::UpdateLidarCallback ret_code:{}, error_key:{}", response->ret_code, response->error_key); if (response->ret_code == 0 && response->error_key == 0) { if (client_data != nullptr) { HapCommandHandler* self = (HapCommandHandler*)client_data; @@ -238,8 +234,8 @@ livox_status HapCommandHandler::SendCommand(const Command &command, const uint16 //LOG_INFO("HapCommandHandler::SendCommand seq:{}, lidar_ip:{}", command.packet.seq_num, command.lidar_ip.c_str()); int byte_send = device_manager_->SendTo(kLivoxLidarTypeIndustrialHAP, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr)); if (byte_send < 0) { - LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}, lidar_port:{}", - byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str(), kLidarCmdPort); + LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}", + byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str()); if (command.cb) { (*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr); } @@ -268,8 +264,8 @@ livox_status HapCommandHandler::SendCommand(const Command &command) { //LOG_INFO("HapCommandHandler::SendCommand seq:{}, lidar_ip:{}", command.packet.seq_num, command.lidar_ip.c_str()); int byte_send = device_manager_->SendTo(kLivoxLidarTypeIndustrialHAP, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr)); if (byte_send < 0) { - LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}, lidar_port:{}", - byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str(), kLidarCmdPort); + LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}", + byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str()); if (command.cb) { (*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr); } @@ -278,12 +274,10 @@ livox_status HapCommandHandler::SendCommand(const Command &command) { return kLivoxLidarStatusSuccess; } - bool HapCommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) { return true; } - } // namespace livox } // namespace lidar diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/hap_command_handler.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/hap_command_handler.h index f437f0d2..4a00b315 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/hap_command_handler.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/hap_command_handler.h @@ -54,7 +54,7 @@ class HapCommandHandler : public CommandHandler { virtual bool Init(std::shared_ptr>& lidars_cfg_ptr, std::shared_ptr>& custom_lidars_cfg_ptr); - virtual void Handle(const uint32_t handle, const Command& command); + virtual void Handle(const uint32_t handle, uint16_t lidar_port, const Command& command); virtual void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info); virtual void UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port); virtual livox_status SendCommand(const Command& command); diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/mid360_command_handler.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/mid360_command_handler.cpp new file mode 100644 index 00000000..77931688 --- /dev/null +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/mid360_command_handler.cpp @@ -0,0 +1,120 @@ +// +// 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 "mid360_command_handler.h" +#include "livox_lidar_def.h" + +#include "base/command_callback.h" + +#include "base/logging.h" +#include "comm/protocol.h" +#include "comm/generate_seq.h" + +#include "build_request.h" +#include "general_command_handler.h" + +#include "parse_lidar_state_info.h" + +namespace livox { + +namespace lidar { + +Mid360CommandHandler::Mid360CommandHandler(DeviceManager* device_manager) + : CommandHandler(device_manager), comm_port_(new CommPort) { +} + + +bool Mid360CommandHandler::Init(bool is_view) { + return true; +} + +bool Mid360CommandHandler::Init(std::shared_ptr>& lidars_cfg_ptr, + std::shared_ptr>& custom_lidars_cfg_ptr) { + + return true; +} + +void Mid360CommandHandler::Handle(const uint32_t handle, uint16_t lidar_port, const Command& command) { + +} + +void Mid360CommandHandler::OnCommandAck(uint32_t handle, const Command &command) { + +} + +void Mid360CommandHandler::OnCommandCmd(uint32_t handle, const uint16_t lidar_port, const Command &command) { + +} + +void Mid360CommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) { + +} + +void Mid360CommandHandler::UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) { + +} + +void Mid360CommandHandler::SetViewLidar(const ViewLidarIpInfo& view_lidar_info) { + +} + +void Mid360CommandHandler::SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg) { + +} + +void Mid360CommandHandler::SetGeneralLidar(const uint32_t handle, const uint16_t lidar_cmd_port) { + +} + +void Mid360CommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, void *client_data) { + +} + +void Mid360CommandHandler::AddDevice(const uint32_t handle) { + +} + +bool Mid360CommandHandler::IsStatusException(const Command &command) { + +} + +livox_status Mid360CommandHandler::SendCommand(const Command &command, const uint16_t lidar_cmd_port) { + +} + +livox_status Mid360CommandHandler::SendCommand(const Command &command) { + + return kLivoxLidarStatusSuccess; +} + + +bool Mid360CommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) { + return true; +} + + +} // namespace livox +} // namespace lidar + diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/mid360_command_handler.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/mid360_command_handler.h new file mode 100644 index 00000000..085ea378 --- /dev/null +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/mid360_command_handler.h @@ -0,0 +1,91 @@ +// +// 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. +// + +#ifndef MID360_COMMAND_HANDLER_H_ +#define MID360_COMMAND_HANDLER_H_ + +#include +#include +#include +#include + +#include "base/command_callback.h" +#include "base/io_thread.h" + +#include "comm/protocol.h" +#include "comm/comm_port.h" +#include "comm/define.h" + +#include "livox_lidar_def.h" +#include "device_manager.h" + +#include "command_handler.h" + +namespace livox { +namespace lidar { + +class Mid360CommandHandler : public CommandHandler { + public: + Mid360CommandHandler(DeviceManager* device_manager); + ~Mid360CommandHandler() {} + virtual bool Init(bool is_view); + + virtual bool Init(std::shared_ptr>& lidars_cfg_ptr, + std::shared_ptr>& custom_lidars_cfg_ptr); + virtual void Handle(const uint32_t handle, uint16_t lidar_port, const Command& command); + virtual void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info); + virtual void UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port); + virtual livox_status SendCommand(const Command& command); + static void UpdateLidarCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data); + void AddDevice(const uint32_t handle); + private: + void SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg); + void SetGeneralLidar(const uint32_t handle, const uint16_t lidar_cmd_port); + void SetViewLidar(const ViewLidarIpInfo& view_lidar_info); + livox_status SendCommand(const Command &command, const uint16_t lidar_cmd_port); + + + bool GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port); + + void CommandsHandle(TimePoint now); + + void OnCommand(uint32_t handle, const Command &command); + void OnCommandAck(const uint32_t handle, const Command &command); + void OnCommandCmd(const uint32_t handle, const uint16_t lidar_port, const Command &command); + bool IsStatusException(const Command &command); + void QueryDiagnosisInfo(uint32_t handle); + void OnLidarInfoChange(const Command &command); + private: + std::unique_ptr comm_port_; + std::mutex device_mutex_; + std::set devices_; + std::map lidars_custom_; + LivoxLidarCfg general_lidar_cfg_; + bool is_view_; +}; + +} // namespace livox +} // namespace lidar + +#endif // MID360_COMMAND_HANDLER_H_ diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/pa_command_handler.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/pa_command_handler.cpp new file mode 100644 index 00000000..b96c15cb --- /dev/null +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/pa_command_handler.cpp @@ -0,0 +1,118 @@ +// +// 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 "pa_command_handler.h" +#include "livox_lidar_def.h" + +#include "base/command_callback.h" + +#include "base/logging.h" +#include "comm/protocol.h" +#include "comm/generate_seq.h" +#include "build_request.h" +#include "general_command_handler.h" + +namespace livox { + +namespace lidar { + +PaCommandHandler::PaCommandHandler(DeviceManager* device_manager) + : CommandHandler(device_manager), comm_port_(new CommPort) { +} + + +bool PaCommandHandler::Init(bool is_view) { + + return true; +} + +bool PaCommandHandler::Init(std::shared_ptr>& lidars_cfg_ptr, + std::shared_ptr>& custom_lidars_cfg_ptr) { + + return true; +} + +void PaCommandHandler::Handle(const uint32_t handle, uint16_t lidar_port, const Command& command) { + +} + +void PaCommandHandler::OnCommandAck(uint32_t handle, const Command &command) { + +} + +void PaCommandHandler::OnCommandCmd(uint32_t handle, const Command &command) { +} + +void PaCommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) { + +} + +void PaCommandHandler::UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) { + +} + +void PaCommandHandler::SetViewLidar(const ViewLidarIpInfo& view_lidar_info) { + +} + +void PaCommandHandler::SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg) { + +} + +void PaCommandHandler::SetGeneralLidar(const uint32_t handle, const uint16_t lidar_cmd_port) { + +} + +void PaCommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle, + LivoxLidarAsyncControlResponse *response, void *client_data) { + +} + +void PaCommandHandler::AddDevice(const uint32_t handle) { + +} + +bool PaCommandHandler::IsStatusException(const Command &command) { + + return true; +} + +livox_status PaCommandHandler::SendCommand(const Command &command, const uint16_t lidar_cmd_port) { + + return kLivoxLidarStatusSuccess; +} + +livox_status PaCommandHandler::SendCommand(const Command &command) { + + return kLivoxLidarStatusSuccess; +} + + +bool PaCommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) { + return true; +} + +} // namespace livox +} // namespace lidar + diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/pa_command_handler.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/pa_command_handler.h new file mode 100644 index 00000000..b5ee6c0d --- /dev/null +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/pa_command_handler.h @@ -0,0 +1,92 @@ +// +// 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. +// + +#ifndef PA_COMMAND_HANDLER_H_ +#define PA_COMMAND_HANDLER_H_ + +#include +#include +#include +#include + +#include "base/command_callback.h" +#include "base/io_thread.h" + +#include "comm/protocol.h" +#include "comm/comm_port.h" +#include "comm/define.h" + +#include "livox_lidar_def.h" +#include "device_manager.h" + +#include "command_handler.h" + +namespace livox { +namespace lidar { + +class PaCommandHandler : public CommandHandler { + public: + PaCommandHandler(DeviceManager* device_manager); + ~PaCommandHandler() {} + static PaCommandHandler& GetInstance(); + virtual bool Init(bool is_view); + + virtual bool Init(std::shared_ptr>& lidars_cfg_ptr, + std::shared_ptr>& custom_lidars_cfg_ptr); + virtual void Handle(const uint32_t handle, uint16_t lidar_port, const Command& command); + virtual void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info); + virtual void UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port); + virtual livox_status SendCommand(const Command& command); + static void UpdateLidarCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data); + void AddDevice(const uint32_t handle); + private: + void SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg); + void SetGeneralLidar(const uint32_t handle, const uint16_t lidar_cmd_port); + void SetViewLidar(const ViewLidarIpInfo& view_lidar_info); + livox_status SendCommand(const Command &command, const uint16_t lidar_cmd_port); + + + bool GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port); + + void CommandsHandle(TimePoint now); + + void OnCommand(uint32_t handle, const Command &command); + void OnCommandAck(uint32_t handle, const Command &command); + void OnCommandCmd(uint32_t handle, const Command &command); + bool IsStatusException(const Command &command); + void QueryDiagnosisInfo(uint32_t handle); + void OnLidarInfoChange(const Command &command); + private: + std::unique_ptr comm_port_; + std::mutex device_mutex_; + std::set devices_; + std::map lidars_custom_; + LivoxLidarCfg general_lidar_cfg_; + bool is_view_; +}; + +} // namespace livox +} // namespace lidar + +#endif // HAP_COMMAND_HANDLER_H_ diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/parse_lidar_state_info.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/parse_lidar_state_info.cpp new file mode 100644 index 00000000..b033366f --- /dev/null +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/parse_lidar_state_info.cpp @@ -0,0 +1,451 @@ + +#include "parse_lidar_state_info.h" +#include "base/logging.h" + +namespace livox { + +namespace lidar { + +bool ParseLidarStateInfo::Parse(const CommPacket& packet, std::string& info_str) { + DirectLidarStateInfo info; + if (!ParseStateInfo(packet, info)) { + return false; + } + + LivoxLidarStateInfoToJson(info, info_str); + return true; +} + +bool ParseLidarStateInfo::ParseStateInfo(const CommPacket& packet, DirectLidarStateInfo& info) { + uint16_t off = 0; + uint16_t key_num = 0; + memcpy(&key_num, &packet.data[off], sizeof(uint16_t)); + off += sizeof(uint16_t) * 2; + + for (uint16_t i = 0; i < key_num; ++i) { + if (off + sizeof(LivoxLidarKeyValueParam) > packet.data_len) { + return false; + } + + LivoxLidarKeyValueParam* kv = (LivoxLidarKeyValueParam*)&packet.data[off]; + off += sizeof(uint16_t); + + uint16_t val_len = 0; + memcpy(&val_len, &packet.data[off], sizeof(uint16_t)); + off += sizeof(uint16_t); + + switch (kv->key) { + case static_cast(kKeyPclDataType) : + memcpy(&info.pcl_data_type, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyPatternMode) : + memcpy(&info.pattern_mode, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyLidarIPCfg) : + ParseLidarIpAddr(packet, off, info); + break; + case static_cast(kKeyStateInfoHostIPCfg) : + ParseStateInfoHostIPCfg(packet, off, info); + break; + case static_cast(kKeyLidarPointDataHostIPCfg) : + ParsePointCloudHostIpCfg(packet, off, info); + break; + case static_cast(kKeyLidarImuHostIPCfg) : + ParseImuDataHostIpCfg(packet, off, info); + break; + case static_cast(kKeyInstallAttitude) : + memcpy(&info.install_attitude, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyRoiCfg0) : + memcpy(&info.roi_cfg0, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyRoiCfg1) : + memcpy(&info.roi_cfg1, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyRoiEn) : + memcpy(&info.roi_en, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyWorkMode) : + memcpy(&info.work_mode, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyImuDataEn) : + memcpy(&info.imu_data_en, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeySn) : + memcpy(info.sn, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyProductInfo) : + memcpy(info.product_info, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyVersionApp) : + memcpy(info.version_app, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyVersionLoader) : + memcpy(info.version_load, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyVersionHardware): + memcpy(info.version_hardware, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyMac) : + memcpy(info.mac, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyCurWorkState) : + memcpy(&info.cur_work_state, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyCoreTemp) : + memcpy(&info.core_temp, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyPowerUpCnt) : + memcpy(&info.powerup_cnt, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyLocalTimeNow) : + memcpy(&info.local_time_now, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyLastSyncTime) : + memcpy(&info.last_sync_time, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyTimeOffset) : + memcpy(&info.time_offset, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyTimeSyncType) : + memcpy(&info.time_sync_type, &packet.data[off], val_len); + off += val_len; + break; + case static_cast(kKeyFwType) : + memcpy(&info.fw_type, &packet.data[off], val_len); + off += val_len; + break; + default : + off += val_len; + } + } + + // printf("Lidar state info, pcl_data_type:%d, pattern_mode:%d, lidar_ip:%s, lidar_submask:%s, lidar_gatway:%s.\n", + // info.pcl_data_type, info.pattern_mode, info.livox_lidar_ip_info.ip_addr, info.livox_lidar_ip_info.net_mask, + // info.livox_lidar_ip_info.gw_addr); + + // printf("Lidar state info, host_ip_addr:%s, host_state_info_port:%u, lidar_state_info_port:%u.\n", + // info.host_state_info.host_ip_addr, info.host_state_info.host_state_info_port, info.host_state_info.lidar_state_info_port); + + // printf("Lidar state info, host_ip_addr:%s, host_point_data_port:%u, lidar_point_data_port:%u.\n", + // info.host_point_ip_info.host_ip_addr, info.host_point_ip_info.host_point_data_port, + // info.host_point_ip_info.lidar_point_data_port); + + // printf("Lidar state info, host_ip_addr:%s, host_imu_data_port:%u, lidar_imu_data_port:%u.\n", + // info.host_imu_data_ip_info.host_ip_addr, info.host_point_ip_info.host_point_data_port, + // info.host_imu_data_ip_info.lidar_imu_data_port); + + // printf("Lidar state info, roll:%f, pitch:%f, yaw:%f, x:%d, y:%d,z:%d.\n", + // info.install_attitude.roll_deg, info.install_attitude.pitch_deg, + // info.install_attitude.yaw_deg, info.install_attitude.x, info.install_attitude.y, info.install_attitude.z); + + // printf("Lidar state info, roi cfg0, yaw_start:%d, yaw_stop:%d, pitch_start:%d, pitch_stop:%d.\n", + // info.roi_cfg0.yaw_start, info.roi_cfg0.yaw_stop, info.roi_cfg0.pitch_start, info.roi_cfg0.pitch_stop); + + // printf("Lidar state info, roi cfg1, yaw_start:%d, yaw_stop:%d, pitch_start:%d, pitch_stop:%d.\n", + // info.roi_cfg1.yaw_start, info.roi_cfg1.yaw_stop, info.roi_cfg1.pitch_start, info.roi_cfg1.pitch_stop); + + // printf("Lidar state info, roi_en:%u, work_mode:%u, imu_data_en:%u, sn:%s, product_info:%s.\n", + // info.roi_en, info.work_mode, info.imu_data_en, info.sn, info.product_info); + + // std::string version_app = std::to_string(info.version_app[0]) + ":" + std::to_string(info.version_app[1]) + ":" + + // std::to_string(info.version_app[2]) + ":" + std::to_string(info.version_app[3]); + + // std::string version_load = std::to_string(info.version_load[0]) + ":" + std::to_string(info.version_load[1]) + ":" + + // std::to_string(info.version_load[2]) + ":" + std::to_string(info.version_load[3]); + + // std::string version_hardware = std::to_string(info.version_hardware[0]) + ":" + std::to_string(info.version_hardware[1]) + ":" + + // std::to_string(info.version_hardware[2]) + ":" + std::to_string(info.version_hardware[3]); + + // std::string mac = std::to_string(info.mac[0]) + ":" + std::to_string(info.mac[1]) + ":" + + // std::to_string(info.mac[2]) + ":" + std::to_string(info.mac[3]) + ":" + + // std::to_string(info.mac[4]) + ":" + std::to_string(info.mac[5]); + + // printf("Lidar state info, version_app:%s, version_load:%s, version_hardware:%s, mac:%s.\n", + // version_app.c_str(), version_load.c_str(), version_hardware.c_str(), mac.c_str()); + + + // printf("Lidar state info, cur_work_state:%u, core_temp:%d, powerup_cnt:%u, local_time_now:%lu, last_sync_time:%lu, time_offset:%ld.\n", + // info.cur_work_state, info.core_temp, info.powerup_cnt, info.local_time_now, info.last_sync_time, info.time_offset); + + // printf("Lidar state info, time_sync_type:%u, fw_type:%u.\n", info.time_sync_type, info.fw_type); + + return true; +} + +void ParseLidarStateInfo::ParseLidarIpAddr(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info) { + uint8_t lidar_ip[4]; + memcpy(lidar_ip, &packet.data[off], sizeof(uint8_t) * 4); + std::string lidar_ip_str = std::to_string(lidar_ip[0]) + "." + std::to_string(lidar_ip[1]) + "." + + std::to_string(lidar_ip[2]) + "." + std::to_string(lidar_ip[3]); + strcpy(info.livox_lidar_ip_info.ip_addr, lidar_ip_str.c_str()); + off += sizeof(uint8_t) * 4; + + uint8_t lidar_submask[4]; + memcpy(lidar_submask, &packet.data[off], sizeof(uint8_t) * 4); + std::string lidar_submask_str = std::to_string(lidar_submask[0]) + "." + std::to_string(lidar_submask[1]) + + "." + std::to_string(lidar_submask[2]) + "." + std::to_string(lidar_submask[3]); + strcpy(info.livox_lidar_ip_info.net_mask, lidar_submask_str.c_str()); + off += sizeof(uint8_t) * 4; + + uint8_t lidar_gateway[4]; + memcpy(lidar_gateway, &packet.data[off], sizeof(uint8_t) * 4); + std::string lidar_gateway_str = std::to_string(lidar_gateway[0]) + "." + std::to_string(lidar_gateway[1]) + + "." + std::to_string(lidar_gateway[2]) + "." + std::to_string(lidar_gateway[3]); + strcpy(info.livox_lidar_ip_info.gw_addr, lidar_gateway_str.c_str()); + off += sizeof(uint8_t) * 4; +} + +void ParseLidarStateInfo::ParseStateInfoHostIPCfg(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info) { + uint8_t host_state_info_ip[4]; + memcpy(host_state_info_ip, &packet.data[off], sizeof(uint8_t) * 4); + std::string host_state_info_ip_str = std::to_string(host_state_info_ip[0]) + "." + + std::to_string(host_state_info_ip[1]) + "." + std::to_string(host_state_info_ip[2]) + "." + + std::to_string(host_state_info_ip[3]); + + strcpy(info.host_state_info.host_ip_addr, host_state_info_ip_str.c_str()); + off += sizeof(uint8_t) * 4; + + memcpy(&info.host_state_info.host_state_info_port, &packet.data[off], sizeof(uint16_t)); + off += sizeof(uint16_t); + + memcpy(&info.host_state_info.lidar_state_info_port, &packet.data[off], sizeof(uint16_t)); + off += sizeof(uint16_t); +} + +void ParseLidarStateInfo::ParsePointCloudHostIpCfg(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info) { + uint8_t host_point_cloud_ip[4]; + memcpy(host_point_cloud_ip, &packet.data[off], sizeof(uint8_t) * 4); + std::string host_point_cloud_ip_str = std::to_string(host_point_cloud_ip[0]) + "." + + std::to_string(host_point_cloud_ip[1]) + "." + std::to_string(host_point_cloud_ip[2]) + "." + + std::to_string(host_point_cloud_ip[3]); + + strcpy(info.host_point_ip_info.host_ip_addr, host_point_cloud_ip_str.c_str()); + off += sizeof(uint8_t) * 4; + + memcpy(&info.host_point_ip_info.host_point_data_port, &packet.data[off], sizeof(uint16_t)); + off += sizeof(uint16_t); + + memcpy(&info.host_point_ip_info.lidar_point_data_port, &packet.data[off], sizeof(uint16_t)); + off += sizeof(uint16_t); +} + +void ParseLidarStateInfo::ParseImuDataHostIpCfg(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info) { + uint8_t host_imu_data_ip[4]; + memcpy(host_imu_data_ip, &packet.data[off], sizeof(uint8_t) * 4); + std::string host_imu_data_ip_str = std::to_string(host_imu_data_ip[0]) + "." + + std::to_string(host_imu_data_ip[1]) + "." + std::to_string(host_imu_data_ip[2]) + "." + + std::to_string(host_imu_data_ip[3]); + + strcpy(info.host_imu_data_ip_info.host_ip_addr, host_imu_data_ip_str.c_str()); + off += sizeof(uint8_t) * 4; + + memcpy(&info.host_imu_data_ip_info.host_imu_data_port, &packet.data[off], sizeof(uint16_t)); + off += sizeof(uint16_t); + + memcpy(&info.host_imu_data_ip_info.lidar_imu_data_port, &packet.data[off], sizeof(uint16_t)); + off += sizeof(uint16_t); +} + +void ParseLidarStateInfo::LivoxLidarStateInfoToJson(const DirectLidarStateInfo& info, std::string& lidar_info) { + rapidjson::StringBuffer buf; + rapidjson::PrettyWriter write(buf); + write.StartObject(); + + write.Key("dev_type"); + write.String("MID360"); + + write.Key("pcl_data_type"); + write.Uint(info.pcl_data_type); + + write.Key("pattern_mode"); + write.Uint(info.pattern_mode); + + write.Key("lidar_ipinfo"); + write.StartObject(); + write.Key("lidar_ipaddr"); + write.String(info.livox_lidar_ip_info.ip_addr); + + write.Key("lidar_subnet_mask"); + write.String(info.livox_lidar_ip_info.net_mask); + + write.Key("lidar_gateway"); + write.String(info.livox_lidar_ip_info.gw_addr); + write.EndObject(); + + write.Key("state_info_host_ipinfo"); + write.StartObject(); + write.Key("host_ip_addr"); + write.String(info.host_state_info.host_ip_addr); + write.Key("host_state_info_port"); + write.Uint(info.host_state_info.host_state_info_port); + write.Key("lidar_state_info_port"); + write.Uint(info.host_state_info.lidar_state_info_port); + write.EndObject(); + + write.Key("point_cloud_host_ipinfo"); + write.StartObject(); + write.Key("host_ip_addr"); + write.String(info.host_point_ip_info.host_ip_addr); + write.Key("host_point_cloud_port"); + write.Uint(info.host_point_ip_info.host_point_data_port); + write.Key("lidar_point_cloud_port"); + write.Uint(info.host_point_ip_info.lidar_point_data_port); + write.EndObject(); + + write.Key("imu_host_ipinfo"); + write.StartObject(); + write.Key("host_ip_addr"); + write.String(info.host_imu_data_ip_info.host_ip_addr); + write.Key("host_imu_data_port"); + write.Uint(info.host_imu_data_ip_info.host_imu_data_port); + write.Key("lidar_imu_data_port"); + write.Uint(info.host_imu_data_ip_info.lidar_imu_data_port); + write.EndObject(); + + write.Key("install_attitude"); + write.StartObject(); + write.Key("roll_deg"); + write.Double(info.install_attitude.roll_deg); + write.Key("pitch_deg"); + write.Double(info.install_attitude.pitch_deg); + write.Key("yaw_deg"); + write.Double(info.install_attitude.yaw_deg); + write.Key("x"); + write.Uint(info.install_attitude.x); + write.Key("y"); + write.Uint(info.install_attitude.y); + write.Key("z"); + write.Uint(info.install_attitude.z); + write.EndObject(); + + write.Key("roi_cfg0"); + write.StartObject(); + write.Key("yaw_start"); + write.Int(info.roi_cfg0.yaw_start); + write.Key("yaw_stop"); + write.Int(info.roi_cfg0.yaw_stop); + write.Key("pitch_start"); + write.Int(info.roi_cfg0.pitch_start); + write.Key("pitch_stop"); + write.Int(info.roi_cfg0.pitch_stop); + write.EndObject(); + + write.Key("roi_cfg1"); + write.StartObject(); + write.Key("yaw_start"); + write.Int(info.roi_cfg0.yaw_start); + write.Key("yaw_stop"); + write.Int(info.roi_cfg0.yaw_stop); + write.Key("pitch_start"); + write.Int(info.roi_cfg0.pitch_start); + write.Key("pitch_stop"); + write.Int(info.roi_cfg0.pitch_stop); + write.EndObject(); + + write.Key("roi_en"); + write.Uint(info.roi_en); + + write.Key("work_mode"); + write.Uint(info.work_mode); + + write.Key("imu_data_en"); + write.Uint(info.imu_data_en); + + write.Key("sn"); + write.String(info.sn); + + write.Key("product_info"); + write.String(info.product_info); + + write.Key("version_app"); + write.StartArray(); + write.Uint(info.version_app[0]); + write.Uint(info.version_app[1]); + write.Uint(info.version_app[2]); + write.Uint(info.version_app[3]); + write.EndArray(); + + write.Key("version_loader"); + write.StartArray(); + write.Uint(info.version_load[0]); + write.Uint(info.version_load[1]); + write.Uint(info.version_load[2]); + write.Uint(info.version_load[3]); + write.EndArray(); + + write.Key("version_hardware"); + write.StartArray(); + write.Uint(info.version_hardware[0]); + write.Uint(info.version_hardware[1]); + write.Uint(info.version_hardware[2]); + write.Uint(info.version_hardware[3]); + write.EndArray(); + + write.Key("mac"); + write.StartArray(); + write.Uint(info.mac[0]); + write.Uint(info.mac[1]); + write.Uint(info.mac[2]); + write.Uint(info.mac[3]); + write.Uint(info.mac[4]); + write.Uint(info.mac[5]); + write.EndArray(); + + write.Key("cur_work_state"); + write.Uint(info.cur_work_state); + + write.Key("core_temp"); + write.Int(info.core_temp); + + write.Key("powerup_cnt"); + write.Uint(info.powerup_cnt); + + write.Key("local_time_now"); + write.Uint64(info.local_time_now); + + write.Key("last_sync_time"); + write.Uint64(info.last_sync_time); + + write.Key("time_offset"); + write.Int64(info.time_offset); + + write.Key("time_sync_type"); + write.Uint(info.time_sync_type); + + write.Key("fw_type"); + write.Uint(info.fw_type); + + write.EndObject(); + + lidar_info = buf.GetString(); + // LOG_INFO("###################################lidar_info_to_json:{}", lidar_info.c_str()); +} + +} // namespace livox +} // namespace direct + + + + + diff --git a/dedicated_sdk/livox_sdk_direct/sdk_core/src/command_handler/parse_lidar_state_info.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/parse_lidar_state_info.h similarity index 64% rename from dedicated_sdk/livox_sdk_direct/sdk_core/src/command_handler/parse_lidar_state_info.h rename to dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/parse_lidar_state_info.h index e4749830..12d7e9dd 100644 --- a/dedicated_sdk/livox_sdk_direct/sdk_core/src/command_handler/parse_lidar_state_info.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/command_handler/parse_lidar_state_info.h @@ -30,33 +30,33 @@ #include #include "comm/define.h" -#include "base/command_callback.h" -#include "base/io_thread.h" -#include - -#include "livox_def_direct.h" - - #include "comm/protocol.h" -#include "command_handler_impl.h" +#include "livox_lidar_def.h" + +#include "third_party/rapidjson/document.h" +#include "third_party/rapidjson/filereadstream.h" +#include "third_party/rapidjson/stringbuffer.h" +#include "third_party/rapidjson/prettywriter.h" namespace livox { -namespace direct { +namespace lidar { class ParseLidarStateInfo { public: - static bool Parse(const CommPacket& packet, DirectLidarStateInfo& info); + static bool Parse(const CommPacket& packet, std::string& info); private: - static void ParseLidarVersionApp(const CommPacket& packet, uint16_t& off, uint16_t val_len, DirectLidarStateInfo& info); - static void ParseLidarMac(const CommPacket& packet, uint16_t& off, uint16_t val_len, DirectLidarStateInfo& info); - static void ParseLidarIpAddr(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info); - static void ParseHostIpAddr(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info); + static bool ParseStateInfo(const CommPacket& packet, DirectLidarStateInfo& info); + static void ParseLidarIpAddr(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info); + static void ParseStateInfoHostIPCfg(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info); + static void ParsePointCloudHostIpCfg(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info); + static void ParseImuDataHostIpCfg(const CommPacket& packet, uint16_t& off, DirectLidarStateInfo& info); + static void LivoxLidarStateInfoToJson(const DirectLidarStateInfo& info, std::string& lidar_info); }; } // namespace livox -} // namespace direct +} // namespace lidar # endif // PARSE_LIDAR_STATE_INFO_H_ diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/data_handler/data_handler.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/data_handler/data_handler.cpp index 0e8560b8..6a48c472 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/data_handler/data_handler.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/data_handler/data_handler.cpp @@ -65,25 +65,11 @@ void DataHandler::Handle(const uint8_t dev_type, const uint32_t handle, uint8_t return; } - // uint32_t data_length = lidar_data->length - sizeof(LivoxLidarEthernetPacket) + 1; - // uint32_t size = 0; if (lidar_data->data_type == kLivoxLidarImuData) { - // size = data_length / sizeof(LivoxLidarImuRawPoint); if (imu_data_callbacks_) { imu_data_callbacks_(handle, dev_type, lidar_data, imu_client_data_); } - } else { - // if (lidar_data->data_type == kLivoxLidarCartesianCoordinateHighData) { - // size = data_length / sizeof(LivoxLidarCartesianHighRawPoint); - // } else if (lidar_data->data_type == kLivoxLidarCartesianCoordinateLowData) { - // size = data_length / sizeof(LivoxLidarCartesianLowRawPoint); - // } else if (lidar_data->data_type == kLivoxLidarSphericalCoordinateData) { - // size = data_length / sizeof(LivoxLidarSpherPoint); - // } else { - // //LOG_INFO("Unknow data type:{}", lidar_data->data_type); - // return; - // } - + } else { if (point_data_callbacks_) { point_data_callbacks_(handle, dev_type, lidar_data, point_client_data_); } diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/device_manager.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/device_manager.cpp index 2b6d06f8..874b310e 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/device_manager.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/device_manager.cpp @@ -28,6 +28,7 @@ #include "comm/generate_seq.h" #include "base/logging.h" +#include "command_handler/command_impl.h" #include "command_handler/general_command_handler.h" #include "data_handler/data_handler.h" @@ -74,6 +75,7 @@ bool DeviceManager::Init(const std::string& host_ip) { bool DeviceManager::Init(std::shared_ptr>& lidars_cfg_ptr, std::shared_ptr>& custom_lidars_cfg_ptr) { + is_view_ = false; lidars_cfg_ptr_ = lidars_cfg_ptr; custom_lidars_cfg_ptr_ = custom_lidars_cfg_ptr; if (lidars_cfg_ptr_ && !(lidars_cfg_ptr_->empty())) { @@ -136,7 +138,6 @@ void DeviceManager::GenerateDevTypeTable() { } } - bool DeviceManager::CreateIOThread() { if (!CreateDetectionIOThread()) { LOG_ERROR("Device manager init failed, create detection io thread failed."); @@ -220,16 +221,13 @@ bool DeviceManager::CreateDetectionChannel() { #ifdef WIN32 #else detection_broadcast_socket_ = util::CreateSocket(kDetectionPort, true, true, true, "255.255.255.255"); - LOG_INFO("Create detection channel detection socket:{}", detection_broadcast_socket_); if (detection_broadcast_socket_ < 0) { LOG_ERROR("Create detection broadcast socket failed."); return false; } detection_io_thread_->loop().lock()->AddDelegate(detection_broadcast_socket_, this, nullptr); #endif - detection_socket_ = util::CreateSocket(kDetectionPort, true, true, true, host_ip_); - LOG_INFO("Create detection channel detection socket:{}", detection_broadcast_socket_); if (detection_socket_ < 0) { LOG_ERROR("Create detection socket failed."); return false; @@ -269,6 +267,19 @@ bool DeviceManager::CreateCommandChannel(const uint8_t dev_type, const HostNetIn return false; } +#ifdef WIN32 +#else + if (dev_type == kLivoxLidarTypeMid360) { + socket_t broadcast_socket = util::CreateSocket(host_net_info.push_msg_port, true, true, true, "255.255.255.255"); + if (broadcast_socket < 0) { + LOG_ERROR("Create broadcast socket failed."); + return false; + } + vec_broadcast_socket_.push_back(broadcast_socket); + cmd_io_thread_->loop().lock()->AddDelegate(broadcast_socket, this, nullptr); + } +#endif + if (!CreateCmdSocketAndAddDelegate(dev_type, host_net_info.log_data_ip, host_net_info.log_data_port, is_custom)) { LOG_ERROR("Create socket and add delegate failed."); return false; @@ -314,21 +325,24 @@ bool DeviceManager::CreateCmdSocketAndAddDelegate(const uint8_t dev_type, const } else { sock = util::CreateSocket(port, true, true, true, host_ip); } + if (sock < 0) { - LOG_ERROR("Add command channel faileld, can not create socket, the ip {} port {} ", host_ip.c_str(), port); + LOG_ERROR("Add command channel faileld, can not create socket, dev_type:{}, the ip {} port {} ", + dev_type, host_ip.c_str(), port); return false; } + socket_vec_.push_back(sock); channel_info_[key] = sock; command_channel_.insert(sock); - if (is_custom) { custom_command_channel_[key] = sock; } else { - general_command_channel_[dev_type] = sock; + if (port == kMid360HostCmdPort || port == kHAPCmdPort) { + general_command_channel_[dev_type] = sock; + } } - cmd_io_thread_->loop().lock()->AddDelegate(sock, this, nullptr); return true; } @@ -370,7 +384,6 @@ void DeviceManager::DetectionLidars() { void DeviceManager::Detection() { uint8_t req_buff[kMaxCommandBufferSize] = {0}; - uint16_t req_len = sizeof(uint8_t); CommPacket packet; packet.protocol = kLidarSdk; @@ -380,9 +393,7 @@ void DeviceManager::Detection() { packet.cmd_type = kCommandTypeCmd; packet.sender_type = kHostSend; packet.data = req_buff; - packet.data_len = req_len; - - //LOG_INFO("Detection lidars, the seq:{}", packet.seq_num); + packet.data_len = 0; std::vector buf(kMaxCommandBufferSize + 1); int size = 0; @@ -420,6 +431,7 @@ void DeviceManager::OnData(socket_t sock, void *client_data) { struct in_addr log_addr; log_addr.s_addr = handle; std::string lidar_ip = inet_ntoa(log_addr); + if (lidar_ip == host_ip_) { return; } @@ -466,7 +478,7 @@ void DeviceManager::OnData(socket_t sock, void *client_data) { return; } } - + if (lidars_cmd_port_.find(dev_type) != lidars_cmd_port_.end()) { if (lidars_cmd_port_[dev_type].find(port) != lidars_cmd_port_[dev_type].end()) { GeneralCommandHandler::GetInstance().Handler(dev_type, handle, port, (uint8_t*)(buf.get()), size); @@ -528,14 +540,12 @@ void DeviceManager::HandleDetectionData(uint32_t handle, DetectionData* detectio return; } return; - } - - lidars_dev_type_[handle] = detection_data->dev_type; + } + lidars_dev_type_[handle] = detection_data->dev_type; } void DeviceManager::GetLivoxLidarInternalInfo(const uint32_t handle) { - GeneralCommandHandler::GetInstance().QueryLivoxLidarInternalInfo(handle, - DeviceManager::GetLivoxLidarInternalInfoCallback, this); + CommandImpl::QueryLivoxLidarInternalInfo(handle, DeviceManager::GetLivoxLidarInternalInfoCallback, this); } void DeviceManager::GetLivoxLidarInternalInfoCallback(livox_status status, uint32_t handle, @@ -596,9 +606,10 @@ void DeviceManager::AddViewLidar(const uint32_t handle, LivoxLidarDiagInternalIn off += kv->length; } - LOG_INFO("host_point_port:{}, lidar_point_port:{}, host_imu_data_port:{}, lidar_imu_data_port:{}", - view_lidar_info_ptr->host_point_port, view_lidar_info_ptr->lidar_point_port, - view_lidar_info_ptr->host_imu_data_port, view_lidar_info_ptr->lidar_imu_data_port); + if (view_lidar_info_ptr->dev_type == kLivoxLidarTypeMid360) { + view_lidar_info_ptr->lidar_point_port = kMid360LidarPointCloudPort; + view_lidar_info_ptr->lidar_imu_data_port = kMid360LidarImuDataPort; + } CreateViewDataChannel(*view_lidar_info_ptr); { @@ -670,7 +681,7 @@ int DeviceManager::SendTo(const uint8_t dev_type, const uint32_t handle, const s sock = detection_socket_; } std::lock_guard lock(mutex_cmd_channel_); - size_t byte = sendto(sock, (const char*)buf.data(), size, 0, addr, addrlen); + size_t byte = sendto(sock, (const char*)buf.data(), size, 0, addr, addrlen); return byte; } @@ -686,16 +697,12 @@ bool DeviceManager::GetCmdChannel(const uint8_t dev_type, const uint32_t handle, } if (general_command_channel_.find(dev_type) != general_command_channel_.end()) { - if (general_command_channel_.find(dev_type) != general_command_channel_.end()) { - sock = general_command_channel_[dev_type]; - return true; - } - return false; + sock = general_command_channel_[dev_type]; + return true; } return false; } - void DeviceManager::Destory() { host_ip_ = ""; @@ -713,6 +720,13 @@ void DeviceManager::Destory() { cmd_io_thread_->loop().lock()->RemoveDelegate(sock, this); } } + + for (auto it = vec_broadcast_socket_.begin(); it != vec_broadcast_socket_.end(); ++it) { + socket_t sock = *it; + if (sock > 0) { + cmd_io_thread_->loop().lock()->RemoveDelegate(sock, this); + } + } for (auto it = data_channel_.begin(); it != data_channel_.end(); ++it) { socket_t sock = *it; @@ -727,6 +741,12 @@ void DeviceManager::Destory() { } socket_vec_.clear(); + for (socket_t & sock : vec_broadcast_socket_) { + util::CloseSock(sock); + sock = -1; + } + vec_broadcast_socket_.clear(); + if (detection_thread_) { is_stop_detection_.store(true); detection_thread_->join(); @@ -777,7 +797,6 @@ void DeviceManager::Destory() { general_command_channel_.clear(); custom_command_channel_.clear(); - command_channel_.clear(); data_channel_.clear(); @@ -810,5 +829,4 @@ DeviceManager::~DeviceManager() { } } // namespace lidar -} // namespace livox - +} // namespace livox \ No newline at end of file diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/device_manager.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/device_manager.h index f5c705c5..c203f8bb 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/device_manager.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/device_manager.h @@ -28,6 +28,9 @@ #include "livox_lidar_def.h" #include "comm/define.h" +#include "comm/comm_port.h" +#include "base/io_thread.h" +#include "base/network/network_util.h" #include #include @@ -37,12 +40,6 @@ #include #include - -#include "base/io_thread.h" -#include "base/network/network_util.h" - -#include "comm/comm_port.h" - #ifdef WIN32 #include #include @@ -55,11 +52,9 @@ #include #endif // WIN32 - namespace livox { namespace lidar { - static const size_t kMaxBufferSize = 8192; const uint8_t kSdkVer = 3; @@ -173,6 +168,7 @@ class DeviceManager : public IOLoop::IOLoopDelegate { std::set command_channel_; std::set data_channel_; + std::vector vec_broadcast_socket_; std::shared_ptr cmd_io_thread_; std::shared_ptr data_io_thread_; @@ -183,8 +179,6 @@ class DeviceManager : public IOLoop::IOLoopDelegate { std::atomic is_stop_detection_{false}; std::shared_ptr detection_thread_; - // std::unique_ptr data_buffers_; - std::mutex lidars_dev_type_mutex_; std::map lidars_dev_type_; diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/livox_lidar_sdk.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/livox_lidar_sdk.cpp index 9bccbbc1..7ad5fcac 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/livox_lidar_sdk.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/livox_lidar_sdk.cpp @@ -22,26 +22,21 @@ // SOFTWARE. // -#include "livox_lidar_sdk.h" +#include "livox_lidar_api.h" #include "livox_lidar_def.h" #include "base/command_callback.h" #include "base/logging.h" - #include "comm/define.h" -// #include "command_handler/command_handler.h" -// #include "data_handler/data_handler.h" - +#include "command_handler/command_impl.h" +#include "command_handler/general_command_handler.h" +#include "data_handler/data_handler.h" #include "parse_cfg_file.h" #include "params_check.h" - #include "device_manager.h" -#include "command_handler/general_command_handler.h" -#include "data_handler/data_handler.h" - #ifdef WIN32 #include #endif // WIN32 @@ -115,9 +110,6 @@ void LivoxLidarSdkUninit() { WSACleanup(); #endif // WIN32 - // command_handler().Destory(); - // data_handler().Destory(); - DeviceManager::GetInstance().Destory(); GeneralCommandHandler::GetInstance().Destory(); DataHandler::GetInstance().Destory(); @@ -154,141 +146,153 @@ void SetLivoxLidarImuDataCallback(LivoxLidarImuDataCallback cb, void* client_dat DataHandler::GetInstance().SetImuDataCallback(cb, client_data); } +void SetLivoxLidarInfoCallback(LivoxLidarInfoCallback cb, void* client_data) { + GeneralCommandHandler::GetInstance().SetLivoxLidarInfoCallback(cb, client_data); +} void SetLivoxLidarInfoChangeCallback(LivoxLidarInfoChangeCallback cb, void* client_data) { GeneralCommandHandler::GetInstance().SetLivoxLidarInfoChangeCallback(cb, client_data); } - livox_status QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().QueryLivoxLidarInternalInfo(handle, cb, client_data); + return CommandImpl::QueryLivoxLidarInternalInfo(handle, cb, client_data); } livox_status SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data) { if (data_type == kLivoxLidarImuData) { return EnableLivoxLidarImuData(handle, cb, client_data); } - return GeneralCommandHandler::GetInstance().SetLivoxLidarPclDataType(handle, data_type, cb, client_data); + return CommandImpl::SetLivoxLidarPclDataType(handle, data_type, cb, client_data); } livox_status SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarScanPattern(handle, scan_pattern, cb, client_data); + return CommandImpl::SetLivoxLidarScanPattern(handle, scan_pattern, cb, client_data); } livox_status SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarDualEmit(handle, enable, cb, client_data); + return CommandImpl::SetLivoxLidarDualEmit(handle, enable, cb, client_data); } livox_status EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().EnableLivoxLidarPointSend(handle, cb, client_data); + return CommandImpl::EnableLivoxLidarPointSend(handle, cb, client_data); } livox_status DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().DisableLivoxLidarPointSend(handle, cb, client_data); + return CommandImpl::DisableLivoxLidarPointSend(handle, cb, client_data); } livox_status SetLivoxLidarIp(uint32_t handle, LivoxLidarIpInfo* ip_config, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarIp(handle, ip_config, cb, client_data); + return CommandImpl::SetLivoxLidarIp(handle, ip_config, cb, client_data); +} + +livox_status SetLivoxLidarStateInfoHostIPCfg(uint32_t handle, HostStateInfoIpInfo* host_state_info_ipcfg, + LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::SetLivoxLidarStateInfoHostIPCfg(handle, *host_state_info_ipcfg, cb, client_data); } livox_status SetLivoxLidarPointDataHostIPCfg(uint32_t handle, HostPointIPInfo* host_point_ipcfg, - LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarPointDataHostIPCfg(handle, *host_point_ipcfg, cb, client_data); + LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::SetLivoxLidarPointDataHostIPCfg(handle, *host_point_ipcfg, cb, client_data); } livox_status SetLivoxLidarImuDataHostIPCfg(uint32_t handle, HostImuDataIPInfo* host_imu_ipcfg, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarImuDataHostIPCfg(handle, *host_imu_ipcfg, cb, client_data); + return CommandImpl::SetLivoxLidarImuDataHostIPCfg(handle, *host_imu_ipcfg, cb, client_data); } livox_status SetLivoxLidarInstallAttitude(uint32_t handle, LivoxLidarInstallAttitude* install_attitude, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarInstallAttitude(handle, *install_attitude, cb, client_data); + return CommandImpl::SetLivoxLidarInstallAttitude(handle, *install_attitude, cb, client_data); } -livox_status SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarBlindSpot(handle, blind_spot, cb, client_data); +livox_status SetLivoxLidarRoiCfg0(uint32_t handle, RoiCfg* roi_cfg0, LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::SetLivoxLidarRoiCfg0(handle, *roi_cfg0, cb, client_data); } -// livox_status SetLivoxLidarFrameRate(uint32_t handle, LivoxLidarPointFrameRate rate, LivoxLidarAsyncControlCallback cb, void* client_data) { -// return GeneralCommandHandler::GetInstance().SetLivoxLidarFrameRate(handle, rate, cb, client_data); -// } +livox_status SetLivoxLidarRoiCfg1(uint32_t handle, RoiCfg* roi_cfg1, LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::SetLivoxLidarRoiCfg1(handle, *roi_cfg1, cb, client_data); +} + +livox_status EnableLivoxLidarRoi(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::EnableLivoxLidarRoi(handle, cb, client_data); +} + +livox_status DisableLivoxLidarRoi(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::DisableLivoxLidarRoi(handle, cb, client_data); +} + +livox_status SetLivoxLidarDetectMode(uint32_t handle, LivoxLidarDetectMode mode, LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::SetLivoxLidarDetectMode(handle, mode, cb, client_data); +} + +livox_status SetLivoxLidarFuncIOCfg(uint32_t handle, FuncIOCfg* func_io_cfg, LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::SetLivoxLidarFuncIOCfg(handle, *func_io_cfg, cb, client_data); +} + +livox_status SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data) { + return CommandImpl::SetLivoxLidarBlindSpot(handle, blind_spot, cb, client_data); +} livox_status SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarWorkMode(handle, work_mode, cb, client_data); + return CommandImpl::SetLivoxLidarWorkMode(handle, work_mode, cb, client_data); } livox_status EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().EnableLivoxLidarGlassHeat(handle, cb, client_data); + return CommandImpl::EnableLivoxLidarGlassHeat(handle, cb, client_data); } livox_status DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().DisableLivoxLidarGlassHeat(handle, cb, client_data); + return CommandImpl::DisableLivoxLidarGlassHeat(handle, cb, client_data); } livox_status SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().SetLivoxLidarGlassHeat(handle, glass_heat, cb, client_data); + return CommandImpl::SetLivoxLidarGlassHeat(handle, glass_heat, cb, client_data); } livox_status EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().EnableLivoxLidarImuData(handle, cb, client_data); + return CommandImpl::EnableLivoxLidarImuData(handle, cb, client_data); } livox_status DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().DisableLivoxLidarImuData(handle, cb, client_data); + return CommandImpl::DisableLivoxLidarImuData(handle, cb, client_data); } livox_status EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().EnableLivoxLidarFusaFunciont(handle, cb, client_data); + return CommandImpl::EnableLivoxLidarFusaFunciont(handle, cb, client_data); } livox_status DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().DisableLivoxLidarFusaFunciont(handle, cb, client_data); + return CommandImpl::DisableLivoxLidarFusaFunciont(handle, cb, client_data); } - - - - -// livox_status SetLivoxLidarLogParam(uint32_t handle, LivoxLidarLogParam* log_param, LivoxLidarAsyncControlCallback cb, void* client_data) { -// return GeneralCommandHandler::GetInstance().SetLivoxLidarLogParam(handle, *log_param, cb, client_data); -// } - - - // reset lidar livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().LivoxLidarRequestReset(handle, cb, client_data); + return CommandImpl::LivoxLidarRequestReset(handle, cb, client_data); } // upgrade livox_status LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().LivoxLidarRequestReboot(handle, cb, client_data); + return CommandImpl::LivoxLidarRequestReboot(handle, cb, client_data); } livox_status LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length, LivoxLidarStartUpgradeCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().LivoxLidarStartUpgrade(handle, data, length, cb, client_data); + return CommandImpl::LivoxLidarStartUpgrade(handle, data, length, cb, client_data); } livox_status LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, LivoxLidarXferFirmwareCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().LivoxLidarXferFirmware(handle, data, length, cb, client_data); + return CommandImpl::LivoxLidarXferFirmware(handle, data, length, cb, client_data); } livox_status LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data, uint16_t length, LivoxLidarCompleteXferFirmwareCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().LivoxLidarCompleteXferFirmware(handle, data, length, cb, client_data); + return CommandImpl::LivoxLidarCompleteXferFirmware(handle, data, length, cb, client_data); } livox_status LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data, uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().LivoxLidarGetUpgradeProgress(handle, data, length, cb, client_data); + return CommandImpl::LivoxLidarGetUpgradeProgress(handle, data, length, cb, client_data); } livox_status LivoxLidarRequestFirmwareInfo(uint32_t handle, LivoxLidarRequestFirmwareInfoCallback cb, void* client_data) { - return GeneralCommandHandler::GetInstance().LivoxLidarRequestFirmwareInfo(handle, cb, client_data); + return CommandImpl::LivoxLidarRequestFirmwareInfo(handle, cb, client_data); } - - - - - diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/params_check.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/params_check.cpp index 2a7ec045..78c0ce0b 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/params_check.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/params_check.cpp @@ -54,6 +54,11 @@ bool ParamsCheck::Check() { return false; } + if (!CheckLidarIp()) { + return false; + } + + CheckLidarPort(); return true; } @@ -86,6 +91,46 @@ bool ParamsCheck::CheckLidarIp() { return true; } +void ParamsCheck::CheckLidarPort() { + for (auto it = lidars_cfg_ptr_->begin(); it != lidars_cfg_ptr_->end(); ++it) { + CheckPort(it->device_type, it->lidar_net_info); + } + + for (auto it = custom_lidars_cfg_ptr_->begin(); it != custom_lidars_cfg_ptr_->end(); ++it) { + CheckPort(it->device_type, it->lidar_net_info); + } +} + +void ParamsCheck::CheckPort(const uint8_t dev_type, LivoxLidarNetInfo& lidar_net_info) { + if (dev_type != kLivoxLidarTypeMid360) { + return; + } + + if (lidar_net_info.cmd_data_port != kMid360LidarCmdPort) { + LOG_ERROR("Mid360 lidar command data port must be {}", kMid360LidarCmdPort); + lidar_net_info.cmd_data_port = kMid360LidarCmdPort; + } + + if (lidar_net_info.push_msg_port != kMid360LidarPushMsgPort) { + LOG_ERROR("Mid360 lidar push msg port must be {}", kMid360LidarPushMsgPort); + lidar_net_info.push_msg_port = kMid360LidarPushMsgPort; + } + + if (lidar_net_info.point_data_port != kMid360LidarPointCloudPort) { + LOG_ERROR("Mid360 lidar point cloud port must be {}", kMid360LidarPointCloudPort); + lidar_net_info.point_data_port = kMid360LidarPointCloudPort; + } + + if (lidar_net_info.imu_data_port != kMid360LidarImuDataPort) { + LOG_ERROR("Mid360 lidar imu data port must be {}", kMid360LidarImuDataPort); + lidar_net_info.imu_data_port = kMid360LidarImuDataPort; + } + + if (lidar_net_info.log_data_port != kMid360LidarLogPort) { + LOG_ERROR("Mid360 lidar log port must be {}", kMid360LidarLogPort); + lidar_net_info.log_data_port = kMid360LidarLogPort; + } +} } // namespace lidar } // namespace livox diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/params_check.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/params_check.h index 0e921cbc..93fb3572 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/params_check.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/params_check.h @@ -44,6 +44,8 @@ class ParamsCheck { bool Check(); private: bool CheckLidarIp(); + void CheckLidarPort(); + void CheckPort(const uint8_t dev_type, LivoxLidarNetInfo& lidar_net_info); private: std::shared_ptr> lidars_cfg_ptr_; std::shared_ptr> custom_lidars_cfg_ptr_; diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/parse_cfg_file.cpp b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/parse_cfg_file.cpp index b717cd16..c9c62e92 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/parse_cfg_file.cpp +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/parse_cfg_file.cpp @@ -53,8 +53,13 @@ bool ParseCfgFile::Parse(std::shared_ptr>& lidars_cfg if (doc.HasMember("HAP") && doc["HAP"].IsObject()) { LivoxLidarCfg hap_lidar_cfg; + if (!GetDevType("HAP", hap_lidar_cfg.device_type)) { + LOG_ERROR("Parse hap object failed, the device_type is error."); + return false; + } + const rapidjson::Value &hap_object = doc["HAP"]; - if (!ParseLidarCfg(hap_object, hap_lidar_cfg)) { + if (!ParseLidarCfg(hap_object, hap_lidar_cfg, false)) { if (raw_file) { std::fclose(raw_file); } @@ -64,13 +69,47 @@ bool ParseCfgFile::Parse(std::shared_ptr>& lidars_cfg lidars_cfg_ptr->push_back(std::move(hap_lidar_cfg)); } + if (doc.HasMember("MID360") && doc["MID360"].IsObject()) { + LivoxLidarCfg lidar_cfg; + if (!GetDevType("MID360", lidar_cfg.device_type)) { + LOG_ERROR("Parse mid360 object failed, the device_type is error."); + return false; + } + const rapidjson::Value &hap_object = doc["MID360"]; + if (!ParseLidarCfg(hap_object, lidar_cfg, false)) { + if (raw_file) { + std::fclose(raw_file); + } + LOG_ERROR("Parse mid360 lidar cfg failed."); + return false; + } + lidars_cfg_ptr->push_back(std::move(lidar_cfg)); + } + + if (doc.HasMember("PA") && doc["PA"].IsObject()) { + LivoxLidarCfg pa_lidar_cfg; + if (!GetDevType("PA", pa_lidar_cfg.device_type)) { + LOG_ERROR("Parse pa object failed, the device_type is error."); + return false; + } + const rapidjson::Value &hap_object = doc["PA"]; + if (!ParseLidarCfg(hap_object, pa_lidar_cfg, false)) { + if (raw_file) { + std::fclose(raw_file); + } + LOG_ERROR("Parse pa lidar cfg failed."); + return false; + } + lidars_cfg_ptr->push_back(std::move(pa_lidar_cfg)); + } + if (doc.HasMember("custom_lidars") && doc["custom_lidars"].IsArray()) { const rapidjson::Value& custom_lidars = doc["custom_lidars"]; size_t lidars_num = custom_lidars.Size(); for (size_t i = 0; i < lidars_num; ++i) { LivoxLidarCfg lidar_cfg; const rapidjson::Value &object = custom_lidars[i]; - if (!ParseLidarCfg(object, lidar_cfg)) { + if (!ParseLidarCfg(object, lidar_cfg, true)) { if (raw_file) { std::fclose(raw_file); } @@ -87,19 +126,21 @@ bool ParseCfgFile::Parse(std::shared_ptr>& lidars_cfg return true; } -bool ParseCfgFile::ParseLidarCfg(const rapidjson::Value &object, LivoxLidarCfg& lidar_cfg) { - if (!object.HasMember("device_type") || !object["device_type"].IsString()) { - LOG_ERROR("Parse hap object failed, has not device_type member or device_type is not string."); - return false; - } +bool ParseCfgFile::ParseLidarCfg(const rapidjson::Value &object, LivoxLidarCfg& lidar_cfg, bool is_custom) { + if (is_custom) { + if (!object.HasMember("device_type") || !object["device_type"].IsString()) { + LOG_ERROR("Parse hap object failed, has not device_type member or device_type is not string."); + return false; + } - std::string device_type = object["device_type"].GetString(); - if (!GetDevType(device_type, lidar_cfg.device_type)) { - LOG_ERROR("Parse hap object failed, the device_type is error, the val:{}", device_type.c_str()); - return false; + std::string device_type = object["device_type"].GetString(); + if (!GetDevType(device_type, lidar_cfg.device_type)) { + LOG_ERROR("Parse hap object failed, the device_type is error, the val:{}", device_type.c_str()); + return false; + } } - if (!ParseLidarNetInfo(object, lidar_cfg.lidar_net_info)) { + if (!ParseLidarNetInfo(object, lidar_cfg.lidar_net_info, is_custom)) { LOG_ERROR("Parse hap lidar net info failed."); return false; } @@ -121,18 +162,26 @@ bool ParseCfgFile::GetDevType(const std::string& type, uint8_t& device_type) { if (type == "HAP") { device_type = kLivoxLidarTypeIndustrialHAP; return true; + } else if (type == "MID360") { + device_type = kLivoxLidarTypeMid360; + return true; + } else if (type == "PA") { + device_type = kLivoxLidarTypePA; + return true; } return false; } - - -bool ParseCfgFile::ParseLidarNetInfo(const rapidjson::Value &object, LivoxLidarNetInfo& lidar_net_info) { - if (!object.HasMember("lidar_ipaddr") || !object["lidar_ipaddr"].IsString()) { - LOG_ERROR("Parse lidar net info failed, has not lidar_ipaddr or lidar_ipaddr is not str."); - return false; +bool ParseCfgFile::ParseLidarNetInfo(const rapidjson::Value &object, LivoxLidarNetInfo& lidar_net_info, bool is_custom) { + if (is_custom) { + if (!object.HasMember("lidar_ipaddr") || !object["lidar_ipaddr"].IsString()) { + LOG_ERROR("Parse lidar net info failed, has not lidar_ipaddr or lidar_ipaddr is not str."); + return false; + } + lidar_net_info.lidar_ipaddr = object["lidar_ipaddr"].GetString(); + } else { + lidar_net_info.lidar_ipaddr = ""; } - lidar_net_info.lidar_ipaddr = object["lidar_ipaddr"].GetString(); if (!object.HasMember("lidar_net_info") || !object["lidar_net_info"].IsObject()) { LOG_ERROR("Parse lidar net info failed, has not lidar_net_info member or lidar_net_info is not object."); @@ -199,11 +248,11 @@ bool ParseCfgFile::ParseHostNetInfo(const rapidjson::Value &object, HostNetInfo& } host_net_info.push_msg_ip = host_net_info_object["push_msg_ip"].GetString(); - if (!host_net_info_object.HasMember("push_msg_ip") || !host_net_info_object["push_msg_ip"].IsString()) { - LOG_ERROR("Parse host net info failed, has not push_msg_ip or push_msg_ip is not string."); + if (!host_net_info_object.HasMember("push_msg_port") || !host_net_info_object["push_msg_port"].IsUint()) { + LOG_ERROR("Parse host net info failed, has not push_msg_port or push_msg_port is not uint."); return false; } - host_net_info.push_msg_ip = host_net_info_object["push_msg_ip"].GetString(); + host_net_info.push_msg_port = host_net_info_object["push_msg_port"].GetUint(); // parse point data info if (!host_net_info_object.HasMember("point_data_ip") || !host_net_info_object["point_data_ip"].IsString()) { diff --git a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/parse_cfg_file.h b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/parse_cfg_file.h index 8298b244..077ba896 100644 --- a/dedicated_sdk/livox_lidar_sdk/sdk_core/src/parse_cfg_file.h +++ b/dedicated_sdk/livox_lidar_sdk/sdk_core/src/parse_cfg_file.h @@ -48,8 +48,8 @@ class ParseCfgFile { std::shared_ptr>& custom_lidars_cfg_ptr); private: bool GetDevType(const std::string& type, uint8_t& device_type); - bool ParseLidarCfg(const rapidjson::Value &object, LivoxLidarCfg& lidar_cfg); - bool ParseLidarNetInfo(const rapidjson::Value &object, LivoxLidarNetInfo& lidar_net_info); + bool ParseLidarCfg(const rapidjson::Value &object, LivoxLidarCfg& lidar_cfg, bool is_custom); + bool ParseLidarNetInfo(const rapidjson::Value &object, LivoxLidarNetInfo& lidar_net_info, bool is_custom); bool ParseHostNetInfo(const rapidjson::Value &object, HostNetInfo& host_net_info); bool ParseGeneralCfgInfo(const rapidjson::Value &object, GeneralCfgInfo& general_cfg_info); private: diff --git a/dedicated_sdk/livox_sdk_common/CMakeLists.txt b/dedicated_sdk/livox_sdk_common/CMakeLists.txt index 7f4dc852..0db2d6a9 100644 --- a/dedicated_sdk/livox_sdk_common/CMakeLists.txt +++ b/dedicated_sdk/livox_sdk_common/CMakeLists.txt @@ -11,7 +11,7 @@ target_include_directories(${SDK_LIBRARY} PUBLIC include include/third_party/FastCRC - include/third_party/spdlog + include/third_party/spdlog PRIVATE src) @@ -53,17 +53,15 @@ target_sources(${SDK_LIBRARY} src/upgrader/industry/industry_upgrader.h src/upgrader/industry/industry_upgrader.cpp - src/upgrader/direct/direct_lidar_upgrader.h - src/upgrader/direct/direct_lidar_upgrader.cpp src/pub_handler/pub_handler.h src/pub_handler/pub_handler.cpp src/pub_handler/lidar_pub_handler.h src/pub_handler/lidar_pub_handler.cpp - + src/algorithm/obstacle_zone_converter/obstacle_zone_converter.h src/algorithm/obstacle_zone_converter/obstacle_zone_converter.cpp) - + set(Eigen3_DIR src/third_party/eigen/share/eigen3/cmake) find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) @@ -73,7 +71,6 @@ target_link_libraries(${SDK_LIBRARY} PUBLIC ${PROJECT_NAME}_static ${PROJECT_NAME}_vehicle_static - ${PROJECT_NAME}_direct_static livox_lidar_sdk_static ) diff --git a/dedicated_sdk/livox_sdk_common/CMakeLists.txt.shared b/dedicated_sdk/livox_sdk_common/CMakeLists.txt.shared index 3d2da46f..8e1eea0f 100644 --- a/dedicated_sdk/livox_sdk_common/CMakeLists.txt.shared +++ b/dedicated_sdk/livox_sdk_common/CMakeLists.txt.shared @@ -9,10 +9,13 @@ add_library(${SDK_LIBRARY} SHARED "") set(LIVOX_SDK_COMMON_PATH ${CMAKE_CURRENT_SOURCE_DIR} CACHE PATH "") target_include_directories(${SDK_LIBRARY} - PUBLIC include + PUBLIC include PRIVATE src) +set_target_properties(${SDK_LIBRARY} PROPERTIES PUBLIC_HEADER "include/livox_sdk_common.h;include/livox_def_common.h;include/livox_sdk_common_util.h") + + target_compile_options(${SDK_LIBRARY} PRIVATE $<$:-Wall -Werror -Wno-c++11-long-long> PRIVATE $<$:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long> @@ -43,8 +46,6 @@ target_sources(${SDK_LIBRARY} src/upgrader/vehicle/vehicle_lidar_upgrader.cpp src/upgrader/industry/industry_upgrader.h src/upgrader/industry/industry_upgrader.cpp - src/upgrader/direct/direct_lidar_upgrader.h - src/upgrader/direct/direct_lidar_upgrader.cpp src/upgrader/livox_lidar/livox_lidar_upgrader.h src/upgrader/livox_lidar/livox_lidar_upgrader.cpp @@ -53,10 +54,10 @@ target_sources(${SDK_LIBRARY} src/pub_handler/pub_handler.cpp src/pub_handler/lidar_pub_handler.h src/pub_handler/lidar_pub_handler.cpp - + src/algorithm/obstacle_zone_converter/obstacle_zone_converter.h src/algorithm/obstacle_zone_converter/obstacle_zone_converter.cpp) - + set(Eigen3_DIR src/third_party/eigen/share/eigen3/cmake) find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) @@ -66,7 +67,6 @@ target_link_libraries(${SDK_LIBRARY} PUBLIC ${PROJECT_NAME}_shared ${PROJECT_NAME}_vehicle_shared - ${PROJECT_NAME}_direct_shared livox_lidar_sdk_shared) install(TARGETS ${SDK_LIBRARY} diff --git a/dedicated_sdk/livox_sdk_common/CMakeLists.txt.static b/dedicated_sdk/livox_sdk_common/CMakeLists.txt.static index 7f4dc852..0db2d6a9 100644 --- a/dedicated_sdk/livox_sdk_common/CMakeLists.txt.static +++ b/dedicated_sdk/livox_sdk_common/CMakeLists.txt.static @@ -11,7 +11,7 @@ target_include_directories(${SDK_LIBRARY} PUBLIC include include/third_party/FastCRC - include/third_party/spdlog + include/third_party/spdlog PRIVATE src) @@ -53,17 +53,15 @@ target_sources(${SDK_LIBRARY} src/upgrader/industry/industry_upgrader.h src/upgrader/industry/industry_upgrader.cpp - src/upgrader/direct/direct_lidar_upgrader.h - src/upgrader/direct/direct_lidar_upgrader.cpp src/pub_handler/pub_handler.h src/pub_handler/pub_handler.cpp src/pub_handler/lidar_pub_handler.h src/pub_handler/lidar_pub_handler.cpp - + src/algorithm/obstacle_zone_converter/obstacle_zone_converter.h src/algorithm/obstacle_zone_converter/obstacle_zone_converter.cpp) - + set(Eigen3_DIR src/third_party/eigen/share/eigen3/cmake) find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIRS}) @@ -73,7 +71,6 @@ target_link_libraries(${SDK_LIBRARY} PUBLIC ${PROJECT_NAME}_static ${PROJECT_NAME}_vehicle_static - ${PROJECT_NAME}_direct_static livox_lidar_sdk_static ) diff --git a/dedicated_sdk/livox_sdk_common/include/livox_sdk_common.h b/dedicated_sdk/livox_sdk_common/include/livox_sdk_common.h index 09d7d6f9..a673f85a 100644 --- a/dedicated_sdk/livox_sdk_common/include/livox_sdk_common.h +++ b/dedicated_sdk/livox_sdk_common/include/livox_sdk_common.h @@ -74,17 +74,6 @@ typedef void (*OnLivoxLidarUpgradeProgressCallback)(LivoxLidarType type, void SetLivoxLidarUpgradeProgressCallback(OnLivoxLidarUpgradeProgressCallback cb, void* client_data); void UpgradeLivoxLidars(const uint32_t* handle, const uint8_t lidar_num); -// Direct lidar upgrade -bool SetDirectUpgradeFirmwarePath(const char* firmware_path); - -typedef void (*OnDirectLidarUpgradeProgressCallback)(LivoxLidarType type, - uint32_t handle, - LidarUpgradeState state, - void *client_data); -void SetDirectLidarUpgradeProgressCallback(OnDirectLidarUpgradeProgressCallback cb, void* client_data); - -void UpgradeDirectLidars(const uint32_t* handle, const uint8_t lidar_num); - bool SetUpgradeFirmwarePath(const char *dir); typedef void (*OnUpgradeProgressCallbackCallback)(LivoxLidarType type, diff --git a/dedicated_sdk/livox_sdk_common/src/livox_sdk_common.cpp b/dedicated_sdk/livox_sdk_common/src/livox_sdk_common.cpp index 1b5c1a81..bc17502c 100644 --- a/dedicated_sdk/livox_sdk_common/src/livox_sdk_common.cpp +++ b/dedicated_sdk/livox_sdk_common/src/livox_sdk_common.cpp @@ -162,19 +162,6 @@ void UpgradeLivoxLidars(const uint32_t* handle, const uint8_t lidar_num) { upgrade_manager().UpgradeLivoxLidars(handle, lidar_num); } -// Direct lidar upgrade -bool SetDirectUpgradeFirmwarePath(const char* firmware_path) { - return upgrade_manager().SetDirectUpgradeFirmwarePath(firmware_path); -} - -void SetDirectLidarUpgradeProgressCallback(OnDirectLidarUpgradeProgressCallback cb, void* client_data) { - upgrade_manager().SetDirectLidarUpgradeProgressCallback(cb, client_data); -} -void UpgradeDirectLidars(const uint32_t* handle, const uint8_t lidar_num) { - upgrade_manager().UpgradeDirectLidars(handle, lidar_num); -} - - bool SetUpgradeFirmwarePath(const char *dir) { return upgrade_manager().SetUpgradeFirmwarePath(std::string(dir)); } diff --git a/dedicated_sdk/livox_sdk_common/src/lvx_handler/lvx_file_converter.cpp b/dedicated_sdk/livox_sdk_common/src/lvx_handler/lvx_file_converter.cpp index 33d3b840..f3de6f00 100644 --- a/dedicated_sdk/livox_sdk_common/src/lvx_handler/lvx_file_converter.cpp +++ b/dedicated_sdk/livox_sdk_common/src/lvx_handler/lvx_file_converter.cpp @@ -30,7 +30,6 @@ #include "livox_def.h" - namespace livox { namespace common { diff --git a/dedicated_sdk/livox_sdk_common/src/lvx_handler/lvx_file_record_handler.cpp b/dedicated_sdk/livox_sdk_common/src/lvx_handler/lvx_file_record_handler.cpp index b888d9cd..7a0f07b9 100644 --- a/dedicated_sdk/livox_sdk_common/src/lvx_handler/lvx_file_record_handler.cpp +++ b/dedicated_sdk/livox_sdk_common/src/lvx_handler/lvx_file_record_handler.cpp @@ -33,7 +33,7 @@ #include "livox_def_common.h" #include "livox_sdk_vehicle.h" #include "livox_sdk.h" -#include "livox_lidar_sdk.h" +#include "livox_lidar_api.h" #include "lvx_file_manager.h" #ifdef WIN32 @@ -415,10 +415,6 @@ void LvxFileRecordHandler::CopyPackHeader(uint32_t handle, return; } - - - - void LvxFileRecordHandler::OnVehicleLidarPointCloudCallback(uint8_t slot, LivoxVehicleEthPacket* data, uint32_t data_num, @@ -602,7 +598,6 @@ void LvxFileRecordHandler::CopyPackHeader(uint8_t slot, return; } - void LvxFileRecordHandler::OnLidarPointCloudCallback(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, diff --git a/dedicated_sdk/livox_sdk_common/src/pub_handler/lidar_pub_handler.cpp b/dedicated_sdk/livox_sdk_common/src/pub_handler/lidar_pub_handler.cpp index f4c8d3a9..58e9e9ae 100644 --- a/dedicated_sdk/livox_sdk_common/src/pub_handler/lidar_pub_handler.cpp +++ b/dedicated_sdk/livox_sdk_common/src/pub_handler/lidar_pub_handler.cpp @@ -37,8 +37,6 @@ void LidarPubHandler::PointCloudProcess(StoragePacket & pkt) { IndustryLidarPointCloudProcess(pkt); } else if (pkt.lidar_type == static_cast(LivoxLidarType::kVehicleLidarType)) { VehicleLidarPointCloudProcess(pkt); - } else if (pkt.lidar_type == static_cast(LivoxLidarType::kDirectLidarType)) { - DirectLidarPointCloudProcess(pkt); } else if (pkt.lidar_type == static_cast(LivoxLidarType::kLivoxLidarType)) { LivoxLidarPointCloudProcess(pkt); } @@ -82,22 +80,6 @@ void LidarPubHandler::VehicleLidarPointCloudProcess(StoragePacket & pkt) { } } -void LidarPubHandler::DirectLidarPointCloudProcess(StoragePacket & pkt) { - switch (pkt.data_type) { - case kCartesianCoordinateHighData: - DirectExtendRawPointCloudProcess(pkt); - break; - case kCartesianCoordinateLowData: - DirectExtendHalfRawPointCloudProcess(pkt); - break; - case kSphericalCoordinateData: - DirectSphericalPointProcess(pkt); - break; - default: - break; - } -} - void LidarPubHandler::LivoxLidarPointCloudProcess(StoragePacket & pkt) { switch (pkt.data_type) { case kLivoxLidarCartesianCoordinateHighData: @@ -428,103 +410,6 @@ void LidarPubHandler::VehicleExtendHalfRawPointCloudProcess(StoragePacket& pkt) } } -void LidarPubHandler::DirectExtendRawPointCloudProcess(StoragePacket& pkt) { - pkt.point_num = pkt.raw_data.size()/ sizeof(LivoxDirectCartesianHighRawPoint); - LivoxDirectCartesianHighRawPoint* raw = (LivoxDirectCartesianHighRawPoint*)pkt.raw_data.data(); - PointCloudXyzlt 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 = 0; - point.tag = 0; - point.offset_time = pkt.time_stamp + i * pkt.point_interval; - std::lock_guard lock(mutex_); - points_clouds_.push_back(point); - } -} - -void LidarPubHandler::DirectExtendHalfRawPointCloudProcess(StoragePacket& pkt) { - pkt.point_num = pkt.raw_data.size()/ sizeof(LivoxDirectCartesianLowRawPoint); - LivoxDirectCartesianLowRawPoint* raw = (LivoxDirectCartesianLowRawPoint*)pkt.raw_data.data(); - PointCloudXyzlt 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::DirectSphericalPointProcess(StoragePacket& pkt) { - pkt.point_num = pkt.raw_data.size()/ sizeof(LivoxDirectSpherPoint); - LivoxDirectSpherPoint* raw = (LivoxDirectSpherPoint*)pkt.raw_data.data(); - PointCloudXyzlt 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 = 0; - point.tag = 0; - point.offset_time = pkt.time_stamp + i * pkt.point_interval; - std::lock_guard lock(mutex_); - points_clouds_.push_back(point); - } -} - void LidarPubHandler::ProcessCartesianHighPoint(StoragePacket & pkt) { LivoxLidarCartesianHighRawPoint* raw = (LivoxLidarCartesianHighRawPoint*)pkt.raw_data.data(); PointCloudXyzlt point = {}; diff --git a/dedicated_sdk/livox_sdk_common/src/pub_handler/lidar_pub_handler.h b/dedicated_sdk/livox_sdk_common/src/pub_handler/lidar_pub_handler.h index 09666516..9a9098bd 100644 --- a/dedicated_sdk/livox_sdk_common/src/pub_handler/lidar_pub_handler.h +++ b/dedicated_sdk/livox_sdk_common/src/pub_handler/lidar_pub_handler.h @@ -5,7 +5,6 @@ #include "livox_def.h" #include "livox_def_common.h" #include "livox_def_vehicle.h" -#include "livox_def_direct.h" #include "livox_sdk_common.h" #include "livox_sdk_common_util.h" #include "livox_lidar_def.h" @@ -42,16 +41,10 @@ class LidarPubHandler { void VehicleExtendRawPointCloudProcess(StoragePacket& pkt); void VehicleExtendHalfRawPointCloudProcess(StoragePacket& pkt); - void DirectLidarPointCloudProcess(StoragePacket & pkt); - void DirectExtendRawPointCloudProcess(StoragePacket& pkt); - void DirectExtendHalfRawPointCloudProcess(StoragePacket& pkt); - void DirectSphericalPointProcess(StoragePacket& pkt); - void LivoxLidarPointCloudProcess(StoragePacket & pkt); void ProcessCartesianHighPoint(StoragePacket & pkt); void ProcessCartesianLowPoint(StoragePacket & pkt); void ProcessSphericalPoint(StoragePacket & pkt); - std::vector points_clouds_; ExtrinsicParameters extrinsic_ = { {0, 0, 0}, diff --git a/dedicated_sdk/livox_sdk_common/src/pub_handler/pub_handler.cpp b/dedicated_sdk/livox_sdk_common/src/pub_handler/pub_handler.cpp index 3f8b8c1c..12c8759c 100644 --- a/dedicated_sdk/livox_sdk_common/src/pub_handler/pub_handler.cpp +++ b/dedicated_sdk/livox_sdk_common/src/pub_handler/pub_handler.cpp @@ -25,10 +25,7 @@ void PubHandler::Uninit() { LidarRemovePointCloudObserver(listen_id_); listen_id_ = 0; } - if (direct_listen_id_ > 0) { - DirectLidarRemovePointCloudObserver(direct_listen_id_); - direct_listen_id_ = 0; - } + if (lidar_listen_id_ > 0) { LivoxLidarRemovePointCloudObserver(lidar_listen_id_); lidar_listen_id_ = 0; @@ -72,7 +69,6 @@ void PubHandler::SetPointCloudsCallback(PointCloudsCallback cb, void* client_dat points_callback_ = cb; vehicle_listen_id_ = VehicleLidarAddPointCloudObserver(OnVehicleLidarPointCloudCallback, this); listen_id_ = LidarAddPointCloudObserver(OnLidarPointCloudCallback, this); - direct_listen_id_ = DirectLidarAddPointCloudObserver(OnDirectLidarPointCloudCallback, this); lidar_listen_id_ = LivoxLidarAddPointCloudObserver(OnLivoxLidarPointCloudCallback, this); } @@ -120,49 +116,6 @@ void PubHandler::OnVehicleLidarPointCloudCallback(uint8_t slot, LivoxVehicleEthP } } -void PubHandler::OnDirectLidarPointCloudCallback(uint32_t handle, LivoxDirectEthPacket* data, uint32_t data_num, void* client_data) { - PubHandler* self = (PubHandler*)client_data; - if (!self) { - return; - } - - // if (data->data_type == kDirectLidarImuData) { - // if (self->imu_callback_) { - // LivoxDirectImuRawPoint* imu = (LivoxDirectImuRawPoint*) data->data; - // LidarImuPoint imu_data; - // imu_data.lidar_type = static_cast(LivoxLidarType::kDirectLidarType); - // imu_data.handle = handle; - // imu_data.time_stamp = GetEthPacketTimestamp(data->timestamp_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, client_data); - // } - // return; - // } - - StoragePacket packet = {}; - packet.handle = handle; - packet.lidar_type = static_cast(LivoxLidarType::kDirectLidarType); - packet.extrinsic_enable = false; - packet.line_num = 1; - packet.data_type = data->data_type; - packet.point_interval = data->time_interval * 1000 / data_num; //ns - - packet.time_stamp = GetDirectEthPacketTimestamp(data->time_type, data->timestamp, sizeof(data->timestamp)); - - uint32_t length = data->length - sizeof(LivoxDirectEthPacket) + 1; - packet.raw_data.insert(packet.raw_data.end(), data->data, data->data + length); - { - std::unique_lock lock(self->packet_mutex_); - self->raw_data_queues_.push_back(packet); - self->packet_condition_.notify_one(); - } -} - void PubHandler::OnLidarPointCloudCallback(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data) { PubHandler* self = (PubHandler*)client_data; if (!self) { @@ -243,7 +196,7 @@ void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t d packet.handle = handle; packet.lidar_type = static_cast(LivoxLidarType::kLivoxLidarType); packet.extrinsic_enable = false; - if (dev_type == kDeviceTypeLidarHAP) { + if (dev_type == kLivoxLidarTypeIndustrialHAP) { packet.line_num = kLineNumberHAP; } else { packet.line_num = kLineNumberDefault; diff --git a/dedicated_sdk/livox_sdk_common/src/pub_handler/pub_handler.h b/dedicated_sdk/livox_sdk_common/src/pub_handler/pub_handler.h index fe1ed66a..45cb6acf 100644 --- a/dedicated_sdk/livox_sdk_common/src/pub_handler/pub_handler.h +++ b/dedicated_sdk/livox_sdk_common/src/pub_handler/pub_handler.h @@ -17,10 +17,8 @@ #include"livox_sdk.h" #include "livox_sdk_vehicle.h" #include "livox_def_vehicle.h" -#include "livox_sdk_direct.h" -#include "livox_def_direct.h" #include "livox_lidar_def.h" -#include "livox_lidar_sdk.h" +#include "livox_lidar_api.h" namespace livox { @@ -63,8 +61,6 @@ class PubHandler { uint32_t data_num, void* client_data); static void OnLidarPointCloudCallback(uint8_t handle, LivoxEthPacket* data, uint32_t data_num, void* client_data); - static void OnDirectLidarPointCloudCallback(uint32_t handle, LivoxDirectEthPacket* data, - uint32_t data_num, void* client_data); static void OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket *data, void *client_data); @@ -90,8 +86,7 @@ class PubHandler { std::map devices_info_; uint16_t vehicle_listen_id_ = 0; uint16_t listen_id_ = 0; - uint16_t direct_listen_id_ = 0; - uint16_t lidar_listen_id_ = 0; // HAP... + uint16_t lidar_listen_id_ = 0; }; PubHandler &pub_handler(); diff --git a/dedicated_sdk/livox_sdk_common/src/upgrader/direct/direct_lidar_upgrader.cpp b/dedicated_sdk/livox_sdk_common/src/upgrader/direct/direct_lidar_upgrader.cpp deleted file mode 100644 index 0249b8bb..00000000 --- a/dedicated_sdk/livox_sdk_common/src/upgrader/direct/direct_lidar_upgrader.cpp +++ /dev/null @@ -1,337 +0,0 @@ -// -// 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 - -#include "direct_lidar_upgrader.h" - -namespace livox { -namespace common { - -typedef int32_t (DirectLidarUpgrader::*FnFsmEvent)(); - -typedef struct { - uint32_t state; - uint32_t event; - FnFsmEvent event_handler; - uint32_t next_state; -} FsmEventTable; - -const FsmEventTable upgrade_event_table[] = { - {kUpgradeIdle, kEventRequestUpgrade, &DirectLidarUpgrader::StartUpgrade, kUpgradeRequest}, - {kUpgradeRequest, kEventRequestUpgrade, &DirectLidarUpgrader::StartUpgrade, kUpgradeRequest}, - {kUpgradeRequest, kEventXferFirmware, &DirectLidarUpgrader::XferFirmware, kUpgradeXferFirmware}, - {kUpgradeXferFirmware, kEventXferFirmware, &DirectLidarUpgrader::XferFirmware, kUpgradeXferFirmware}, - {kUpgradeXferFirmware, kEventCompleteXferFirmware, &DirectLidarUpgrader::CompleteXferFirmware, kUpgradeCompleteXferFirmware}, - {kUpgradeCompleteXferFirmware, kEventCompleteXferFirmware, &DirectLidarUpgrader::CompleteXferFirmware, kUpgradeCompleteXferFirmware}, - {kUpgradeCompleteXferFirmware, kEventGetUpgradeProgress, &DirectLidarUpgrader::GetUpgradeProgress, kUpgradeGetUpgradeProgress}, - {kUpgradeGetUpgradeProgress, kEventGetUpgradeProgress, &DirectLidarUpgrader::GetUpgradeProgress, kUpgradeGetUpgradeProgress}, - {kUpgradeGetUpgradeProgress, kEventComplete, &DirectLidarUpgrader::UpgradeComplete, kUpgradeComplete}, - {kUpgradeComplete, kEventComplete, &DirectLidarUpgrader::UpgradeComplete, kUpgradeComplete}, - {kUpgradeComplete, kEventReinit, nullptr, kUpgradeIdle} - // {kUpgradeRebootDevice, kEventReinit, nullptr, kUpgradeIdle}, -}; - -DirectLidarUpgrader::DirectLidarUpgrader(const Firmware& firmware, const uint32_t slot) - : firmware_(firmware), read_offset_(0), read_length_(1024), slot_(slot), fsm_state_(0), - upgrade_error_(0), progress_(0), try_count_(0) {} - -DirectLidarUpgrader::~DirectLidarUpgrader() { - while (true) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - if (IsUpgradeError()) { - printf("Direct lidar[%d] upgrade error, try again please!\r\n", slot_); - break; - } - - if (IsUpgradeComplete()) { - printf("Direct lidar[%d] upgrade successfully.\r\n", slot_); - break; - } - } - - if (upgrade_thread_ && upgrade_thread_->joinable()) { - upgrade_thread_->join(); - upgrade_thread_ = nullptr; - } -} - -void DirectLidarUpgrader::AddUpgradeProgressObserver(DirectLidarUpgradeProgressCallback observer) { - observer_ = observer; -} - -bool DirectLidarUpgrader::StartUpgradeDirectLidar() { - upgrade_thread_ = std::make_shared([this](){ - this->FsmEventHandler(kEventRequestUpgrade, 10); - }); - return true; -} - -void DirectLidarUpgrader::FsmEventHandler(FsmEvent event, uint8_t progress) { - FnFsmEvent event_handler = nullptr; - if ((event == kEventTimeout) || (event == kEventErr)) { - FsmStateChange(event); - } - printf("Fsm event handler, the direct_lidar[%d] State[%d] | Event[%d]\r\n", slot_, fsm_state_, event); - for (uint32_t i = 0; i < sizeof(upgrade_event_table) / sizeof(upgrade_event_table[0]); i++) { - if ((fsm_state_ == upgrade_event_table[i].state) && (event == upgrade_event_table[i].event)) { - event_handler = upgrade_event_table[i].event_handler; - fsm_state_ = upgrade_event_table[i].next_state; - printf("Fsm event handler, the direct_lidar[%d] New State[%d] | Event[%d]\r\n", slot_, fsm_state_, event); - break; - } - } - - if (event_handler) { - (this->*event_handler)(); - } - - if (observer_) { - LidarUpgradeState upgrade_state = {event, progress}; - observer_(slot_, upgrade_state); - } -} - -livox_direct_status DirectLidarUpgrader::StartUpgrade() { - read_offset_ = 0; - upgrade_error_ = 0; - progress_ = 0; - - uint8_t request_buf[1024] = { 0 }; - DirectStartUpgradeRequest *request = (DirectStartUpgradeRequest *)request_buf; - request->firmware_type = firmware_.header_.firmware_type; - request->firmware_length = firmware_.header_.firmware_length; - request->encrypt_type = firmware_.header_.encrypt_type; - request->dev_type = firmware_.header_.device_type; - return DirectLidarStartUpgrade(slot_, request_buf, sizeof(DirectStartUpgradeRequest), StartUpgradeResponseHandler, this); -} - -livox_direct_status DirectLidarUpgrader::XferFirmware() { - uint8_t send_bufer[2048]; - DirectXferFirmwareResquest* request = (DirectXferFirmwareResquest*)send_bufer; - uint32_t firmware_length = firmware_.header_.firmware_length; - uint32_t read_length = read_length_; - - if (read_offset_ < firmware_length) { - if (read_length > (firmware_length - read_offset_)) { - read_length = firmware_length - read_offset_; - } - } else { - printf("The direct_lidar[%d] xfer firmware failed, Read_offset is err, firmware_length[%d], read_offset[%d].\r\n", - slot_, firmware_length, read_offset_); - return -1; - } - - memcpy(request->data, &firmware_.data_[read_offset_], read_length); - request->offset = read_offset_; - request->length = read_length; - request->encrypt_type = firmware_.header_.encrypt_type; - // read_offset_ += read_length; - printf("The direct_lidar[%d] xfer firmware read offset %d\r\n", slot_, request->offset); - - return DirectLidarXferFirmware(slot_, (uint8_t *)request, - sizeof(DirectXferFirmwareResquest) + read_length - sizeof(request->data), - XferFirmwareResponseHandler, this); -} - -livox_direct_status DirectLidarUpgrader::CompleteXferFirmware() { - uint8_t send_bufer[2048]; - DirectCompleteXferFirmwareResquest* request = (DirectCompleteXferFirmwareResquest*)send_bufer; - - request->checksum_type = firmware_.header_.checksum_type; - request->checksum_length = firmware_.header_.checksum_length; - memcpy(request->checksum, firmware_.header_.checksum, request->checksum_length); - - return DirectLidarCompleteXferFirmware(slot_, (uint8_t *)request, - sizeof(DirectCompleteXferFirmwareResquest) + - request->checksum_length - sizeof(request->checksum), - CompleteXferFirmwareResponseHandler, this); -} - -livox_direct_status DirectLidarUpgrader::GetUpgradeProgress() { - return DirectLidarGetUpgradeProgress(slot_, nullptr, 0, GetProgressResponseHandler, this); -} - -livox_direct_status DirectLidarUpgrader::UpgradeComplete() { - return DirectLidarRequestReboot(slot_, UpgradeCompleteResponseHandler, this); -} - -int32_t DirectLidarUpgrader::FsmStateChange(FsmEvent event) { - if (event < kEventUndef) { - fsm_state_ = event; - } - return 0; -} - -/** - * Upgrade Callback handler - */ -void DirectLidarUpgrader::StartUpgradeResponseHandler(livox_direct_status status, uint32_t slot, - DirectStartUpgradeResponse* response, void* client_data) { - DirectLidarUpgrader* upgrade = static_cast(client_data); - - if (status == kDirectStatusSuccess) { - upgrade->try_count_ = 0; - printf("Start upgrade the direct_lidar[%d], ret_code[%d]!\r\n", slot, response->ret_code); - if (response->ret_code) { - if (response->ret_code == kSystemIsNotReady) { - printf("Start upgrade failed, the direct_lidar[%d] is busy, try again!\r\n", slot); - upgrade->FsmEventHandler(kEventRequestUpgrade, 10); - } else { - printf("Start upgrade failed, the direct_lidar[%d] ret_code[%d], try again!\r\n", slot, response->ret_code); - upgrade->FsmEventHandler(kEventErr, 100); - } - } else { - printf("Start upgrade succ, the direct_lidar[%d] start to xfer data!\r\n", slot); - upgrade->FsmEventHandler(kEventXferFirmware, 20); - } - } else { - printf("Start upgrade failed, the direct_lidar[%d] start upgrade is timeout[%d], try again!\r\n", slot, upgrade->try_count_); - ++upgrade->try_count_; - if (upgrade->try_count_ < kGeneralTryCountLimit) { - upgrade->FsmEventHandler(kEventRequestUpgrade, 10); - } else { - upgrade->try_count_ = 0; - upgrade->FsmEventHandler(kEventErr, 100); - printf("Start upgrade failed, the direct_lidar[%d] start upgrade exceed limit! exit!\r\n", slot); - } - } -} - -void DirectLidarUpgrader::XferFirmwareResponseHandler(livox_direct_status status, uint32_t slot, - DirectXferFirmwareResponse* response, void* client_data) { - DirectLidarUpgrader* upgrade = static_cast(client_data); - - if (status == kDirectStatusSuccess) { - upgrade->try_count_ = 0; - if (response->ret_code) { - printf("The direct_lidar[%d] Xfer firmware fail[%d]\r\n", slot, response->ret_code); - upgrade->FsmEventHandler(kEventErr, 100); - } else { - upgrade->read_offset_ += upgrade->read_length_; - if (upgrade->read_offset_ < upgrade->firmware_.header_.firmware_length) { - upgrade->FsmEventHandler(kEventXferFirmware, 20); - } else { - printf("Xfer firmware succ, the direct_lidar[%d] last offset[%d]\r\n", slot, upgrade->read_offset_); - upgrade->FsmEventHandler(kEventCompleteXferFirmware, 40); - } - } - } else { - printf("Xfer firmware failed, the direct_lidar[%d] xfer firmware timeout, try_count:%d, try again!\r\n", slot, upgrade->try_count_); - ++upgrade->try_count_; - if (upgrade->try_count_ < kGeneralTryCountLimit) { - upgrade->FsmEventHandler(kEventXferFirmware, 20); - } else { - upgrade->try_count_ = 0; - upgrade->FsmEventHandler(kEventErr, 100); - printf("Xfer firmware failed, the direct_lidar[%d] exceed limit! exit!\r\n", slot); - } - } -} - -void DirectLidarUpgrader::CompleteXferFirmwareResponseHandler(livox_direct_status status, uint32_t slot, - DirectCompleteXferFirmwareResponse* response, void* client_data) { - DirectLidarUpgrader* upgrade = static_cast(client_data); - - if (status == kDirectStatusSuccess) { - upgrade->try_count_ = 0; - if (response->ret_code) { - printf("Complete xfer failed, the direct_lidar[%d] ret_code:%d.\r\n", slot, response->ret_code); - upgrade->FsmEventHandler(kEventErr, 100); - } else { - printf("The direct_lidar[%d] complete xfer succ.\n", slot); - upgrade->FsmEventHandler(kEventGetUpgradeProgress, 50); - } - } else { - printf("Complete xfer failed, the direct_lidar[%d] complete xfer is timeout, try_count:%d, try again!\r\n", slot, upgrade->try_count_); - ++upgrade->try_count_; - if (upgrade->try_count_ < kGeneralTryCountLimit) { - upgrade->FsmEventHandler(kEventCompleteXferFirmware, 50); - } else { - upgrade->try_count_ = 0; - upgrade->FsmEventHandler(kEventErr, 100); - printf("Complete xfer failed, the direct_lidar[%d] complete xfer exceed limit! exit!\r\n", slot); - } - } -} - -void DirectLidarUpgrader::GetProgressResponseHandler(livox_direct_status status, uint32_t slot, - DirectGetUpgradeProgressResponse* response, void* client_data) { - DirectLidarUpgrader* upgrade = static_cast(client_data); - if (status == kDirectStatusSuccess) { - upgrade->try_count_ = 0; - if (response->ret_code) { - printf("Get progress failed, the direct_lidar[%d] ret_code:%d.\r\n", slot, response->ret_code); - upgrade->FsmEventHandler(kEventErr, 100); - } else { - if (response->progress < 100) { - printf("The direct_lidar[%d] get progress[%d]\r\n", slot, response->progress); - upgrade->FsmEventHandler(kEventGetUpgradeProgress, response->progress/2 + 50); - } else { - printf("The direct_lidar[%d] Get progress[%d]\r\n", slot, response->progress); - upgrade->FsmEventHandler(kEventComplete, 100); - } - } - } else { - printf("Get progress failed, the direct_lidar[%d] get progress timeout, try_count:%d, try again!\r\n", slot, upgrade->try_count_); - ++upgrade->try_count_; - if (upgrade->try_count_ < kGeneralTryCountLimit) { - upgrade->FsmEventHandler(kEventGetUpgradeProgress, upgrade->progress_/2 + 50); - } else { - upgrade->try_count_ = 0; - upgrade->FsmEventHandler(kEventErr, 100); - printf("Get progress failed, the direct_lidar[%d] get progress exceed limit! exit!\r\n", slot); - } - } -} - -void DirectLidarUpgrader::UpgradeCompleteResponseHandler(livox_direct_status status, uint32_t slot, - DirectRebootResponse* response, void* client_data) { - DirectLidarUpgrader* upgrade = static_cast(client_data); - - if (status == kDirectStatusSuccess) { - upgrade->try_count_ = 0; - if (response->ret_code) { - printf("Upgrade complete failed, the direct_lidar[%d] reboot device fail!\r\n", slot); - upgrade->FsmEventHandler(kEventErr, 100); - } else { - printf("The direct_lidar[%d] upgrade complete succ.\n", slot); - upgrade->FsmEventHandler(kEventReinit, 100); - } - } else { - printf("Upgrade complete failed, the direct_lidar[%d] reboot device timeout, try_count:%d, try again!\r\n", - slot, upgrade->try_count_); - ++upgrade->try_count_; - if (upgrade->try_count_ < kGeneralTryCountLimit) { - upgrade->FsmEventHandler(kEventComplete, 100); - } else { - upgrade->try_count_ = 0; - upgrade->FsmEventHandler(kEventReinit, 100); - printf("Upgrade complete failed, the direct_lidar[%d] reboot device exceed limit! exit!\r\n", slot); - } - } -} - -} // namespace comm -} // namespace livox diff --git a/dedicated_sdk/livox_sdk_common/src/upgrader/direct/direct_lidar_upgrader.h b/dedicated_sdk/livox_sdk_common/src/upgrader/direct/direct_lidar_upgrader.h deleted file mode 100644 index 49169187..00000000 --- a/dedicated_sdk/livox_sdk_common/src/upgrader/direct/direct_lidar_upgrader.h +++ /dev/null @@ -1,96 +0,0 @@ -// -// 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. -// - -#ifndef LIVOX_DIRECT_LIDAR_UPGRADER_H_ -#define LIVOX_DIRECT_LIDAR_UPGRADER_H_ - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "../firmware/firmware.h" - -namespace livox { -namespace common { - -class DirectLidarUpgrader { - public: - using DirectLidarUpgradeProgressCallback = std::function; - - - DirectLidarUpgrader(const Firmware& firmware, const uint32_t slot); - ~DirectLidarUpgrader(); - - void AddUpgradeProgressObserver(DirectLidarUpgradeProgressCallback observer); - bool StartUpgradeDirectLidar(); - - livox_direct_status StartUpgrade(); - livox_direct_status XferFirmware(); - livox_direct_status CompleteXferFirmware(); - livox_direct_status GetUpgradeProgress(); - livox_direct_status UpgradeComplete(); - - static void StartUpgradeResponseHandler(livox_direct_status status, uint32_t slot, - DirectStartUpgradeResponse* response, void* client_data); - static void XferFirmwareResponseHandler(livox_direct_status status, uint32_t slot, - DirectXferFirmwareResponse* response, void* client_data); - static void CompleteXferFirmwareResponseHandler(livox_direct_status status, uint32_t slot, - DirectCompleteXferFirmwareResponse* response, void* client_data); - static void GetProgressResponseHandler(livox_direct_status status, uint32_t slot, - DirectGetUpgradeProgressResponse* response, void* client_data); - static void UpgradeCompleteResponseHandler(livox_direct_status status, uint32_t slot, - DirectRebootResponse* response, void* client_data); - - int32_t FsmStateChange(FsmEvent event); - void FsmEventHandler(FsmEvent event, uint8_t progress); - - bool IsUpgradeComplete() { return (fsm_state_ == kUpgradeIdle); } - bool IsUpgradeError() { return (fsm_state_ == kUpgradeTimeout) || (fsm_state_ == kUpgradeErr); } - - private: - const Firmware& firmware_; - uint32_t read_offset_; - uint32_t read_length_; - uint32_t slot_; - uint32_t fsm_state_; - - uint32_t upgrade_error_; - uint8_t progress_; - - uint32_t try_count_; - std::shared_ptr upgrade_thread_; - DirectLidarUpgradeProgressCallback observer_; - -}; - -} // namespace comm -} // namespace LIVOX_VEHICLE_LIDAR_UPGRADER_H_ - -#endif diff --git a/dedicated_sdk/livox_sdk_common/src/upgrader/livox_lidar/livox_lidar_upgrader.h b/dedicated_sdk/livox_sdk_common/src/upgrader/livox_lidar/livox_lidar_upgrader.h index 0ac8f6bd..55e3d380 100644 --- a/dedicated_sdk/livox_sdk_common/src/upgrader/livox_lidar/livox_lidar_upgrader.h +++ b/dedicated_sdk/livox_sdk_common/src/upgrader/livox_lidar/livox_lidar_upgrader.h @@ -33,7 +33,7 @@ #include #include -#include +#include #include "../firmware/firmware.h" diff --git a/dedicated_sdk/livox_sdk_common/src/upgrader/upgrade_manager.cpp b/dedicated_sdk/livox_sdk_common/src/upgrader/upgrade_manager.cpp index 5052bbfb..cd6fdaeb 100644 --- a/dedicated_sdk/livox_sdk_common/src/upgrader/upgrade_manager.cpp +++ b/dedicated_sdk/livox_sdk_common/src/upgrader/upgrade_manager.cpp @@ -25,7 +25,6 @@ #include "upgrade_manager.h" #include #include -#include "direct/direct_lidar_upgrader.h" namespace livox { namespace common { @@ -123,50 +122,6 @@ void UpgradeManager::CloseVehicleLidarFirmwareFile() { vehicle_firmware_.Close(); } -//Direct lidar upgrader -bool UpgradeManager::SetDirectUpgradeFirmwarePath(const char* firmware_path) { - if (!direct_firmware_.Open(firmware_path)) { - printf("Open firmware_path fail\r\n"); - return false; - } - - return true; -} - -void UpgradeManager::SetDirectLidarUpgradeProgressCallback(OnDirectLidarUpgradeProgressCallback cb, void* client_data) { - direct_lidar_info_cb_ = cb; - direct_lidar_client_data_ = client_data; -} - -void UpgradeManager::UpgradeDirectLidars(const uint32_t* handle, const uint8_t lidar_num) { - std::vector upgrader_vec; - upgrader_vec.reserve(lidar_num); - for (size_t i = 0; i < lidar_num; ++i) { - DirectLidarUpgrader upgrader(direct_firmware_, handle[i]); - - OnDirectLidarUpgradeProgressCallback cb = direct_lidar_info_cb_; - void* client_data = direct_lidar_client_data_; - upgrader.AddUpgradeProgressObserver([cb, client_data](uint32_t slot, LidarUpgradeState state) { - if (cb) { - cb(LivoxLidarType::kDirectLidarType, slot, state, client_data); - } - }); - - upgrader_vec.emplace_back(std::move(upgrader)); - } - - for (size_t i = 0; i < upgrader_vec.size(); ++i) { - DirectLidarUpgrader& upgrader = upgrader_vec[i]; - upgrader.StartUpgradeDirectLidar(); - } - - CloseDirectLidarFirmwareFile(); -} - -void UpgradeManager::CloseDirectLidarFirmwareFile() { - direct_firmware_.Close(); -} - //livoix lidar upgrader bool UpgradeManager::SetLivoxLidarUpgradeFirmwarePath(const char* firmware_path) { if (!livox_lidar_firmware_.Open(firmware_path)) { diff --git a/dedicated_sdk/livox_sdk_common/src/upgrader/upgrade_manager.h b/dedicated_sdk/livox_sdk_common/src/upgrader/upgrade_manager.h index 884bef72..56cb1e33 100644 --- a/dedicated_sdk/livox_sdk_common/src/upgrader/upgrade_manager.h +++ b/dedicated_sdk/livox_sdk_common/src/upgrader/upgrade_manager.h @@ -41,8 +41,6 @@ #include "vehicle/vehicle_upgrader.h" #include "vehicle/vehicle_lidar_upgrader.h" -#include "direct/direct_lidar_upgrader.h" - #include "industry/industry_upgrader.h" #include "livox_lidar/livox_lidar_upgrader.h" @@ -102,15 +100,11 @@ class UpgradeManager { // for vehicle Firmware vehicle_firmware_; - Firmware direct_firmware_; Firmware livox_lidar_firmware_; OnUpgradeProgressCallbackCallback info_cb_; void *client_data_; - OnDirectLidarUpgradeProgressCallback direct_lidar_info_cb_; - void *direct_lidar_client_data_; - OnLivoxLidarUpgradeProgressCallback livox_lidar_info_cb_; void *livox_lidar_client_data_; }; diff --git a/dedicated_sdk/livox_sdk_direct/CMakeLists.txt b/dedicated_sdk/livox_sdk_direct/CMakeLists.txt deleted file mode 100644 index 77320f07..00000000 --- a/dedicated_sdk/livox_sdk_direct/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 3.0) - -project(livox_sdk) - -set(CMAKE_CXX_STANDARD 11) - -message(STATUS "main project dir: " ${PROJECT_SOURCE_DIR}) - -if (CMAKE_CROSSCOMPILING) - set(THREADS_PTHREAD_ARG - "PLEASE_FILL_OUT-FAILED_TO_RUN" - CACHE STRING "Result from TRY_RUN" FORCE) -endif() - -if (UNIX) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") -endif(UNIX) - -add_subdirectory(sdk_core sdk_core) diff --git a/dedicated_sdk/livox_sdk_direct/Doxyfile b/dedicated_sdk/livox_sdk_direct/Doxyfile deleted file mode 100644 index dc4d1d8f..00000000 --- a/dedicated_sdk/livox_sdk_direct/Doxyfile +++ /dev/null @@ -1,2567 +0,0 @@ -# Doxyfile 1.8.16 - -# This file describes the settings to be used by the documentation system -# doxygen (www.doxygen.org) for a project. -# -# All text after a double hash (##) is considered a comment and is placed in -# front of the TAG it is preceding. -# -# All text after a single hash (#) is considered a comment and will be ignored. -# The format is: -# TAG = value [value, ...] -# For lists, items can also be appended using: -# TAG += value [value, ...] -# Values that contain spaces should be placed between quotes (\" \"). - -#--------------------------------------------------------------------------- -# Project related configuration options -#--------------------------------------------------------------------------- - -# This tag specifies the encoding used for all characters in the configuration -# file that follow. The default is UTF-8 which is also the encoding used for all -# text before the first occurrence of this tag. Doxygen uses libiconv (or the -# iconv built into libc) for the transcoding. See -# https://www.gnu.org/software/libiconv/ for the list of possible encodings. -# The default value is: UTF-8. - -DOXYFILE_ENCODING = UTF-8 - -# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by -# double-quotes, unless you are using Doxywizard) that should identify the -# project for which the documentation is generated. This name is used in the -# title of most generated pages and in a few other places. -# The default value is: My Project. - -PROJECT_NAME = "Livox SDK API" - -# The PROJECT_NUMBER tag can be used to enter a project or revision number. This -# could be handy for archiving the generated documentation or if some version -# control system is used. - -PROJECT_NUMBER = V2.2.0 - -# Using the PROJECT_BRIEF tag one can provide an optional one line description -# for a project that appears at the top of each page and should give viewer a -# quick idea about the purpose of the project. Keep the description short. - -PROJECT_BRIEF = - -# With the PROJECT_LOGO tag one can specify a logo or an icon that is included -# in the documentation. The maximum height of the logo should not exceed 55 -# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy -# the logo to the output directory. - -PROJECT_LOGO = doc/images/1.png - -# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path -# into which the generated documentation will be written. If a relative path is -# entered, it will be relative to the location where doxygen was started. If -# left blank the current directory will be used. - -OUTPUT_DIRECTORY = docs - -# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- -# directories (in 2 levels) under the output directory of each output format and -# will distribute the generated files over these directories. Enabling this -# option can be useful when feeding doxygen a huge amount of source files, where -# putting all generated files in the same directory would otherwise causes -# performance problems for the file system. -# The default value is: NO. - -CREATE_SUBDIRS = NO - -# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII -# characters to appear in the names of generated files. If set to NO, non-ASCII -# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode -# U+3044. -# The default value is: NO. - -ALLOW_UNICODE_NAMES = NO - -# The OUTPUT_LANGUAGE tag is used to specify the language in which all -# documentation generated by doxygen is written. Doxygen will use this -# information to generate all constant output in the proper language. -# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, -# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), -# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, -# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), -# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, -# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, -# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, -# Ukrainian and Vietnamese. -# The default value is: English. - -OUTPUT_LANGUAGE = English - -# The OUTPUT_TEXT_DIRECTION tag is used to specify the direction in which all -# documentation generated by doxygen is written. Doxygen will use this -# information to generate all generated output in the proper direction. -# Possible values are: None, LTR, RTL and Context. -# The default value is: None. - -OUTPUT_TEXT_DIRECTION = None - -# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member -# descriptions after the members that are listed in the file and class -# documentation (similar to Javadoc). Set to NO to disable this. -# The default value is: YES. - -BRIEF_MEMBER_DESC = YES - -# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief -# description of a member or function before the detailed description -# -# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the -# brief descriptions will be completely suppressed. -# The default value is: YES. - -REPEAT_BRIEF = YES - -# This tag implements a quasi-intelligent brief description abbreviator that is -# used to form the text in various listings. Each string in this list, if found -# as the leading text of the brief description, will be stripped from the text -# and the result, after processing the whole list, is used as the annotated -# text. Otherwise, the brief description is used as-is. If left blank, the -# following values are used ($name is automatically replaced with the name of -# the entity):The $name class, The $name widget, The $name file, is, provides, -# specifies, contains, represents, a, an and the. - -ABBREVIATE_BRIEF = "The $name class" \ - "The $name widget" \ - "The $name file" \ - is \ - provides \ - specifies \ - contains \ - represents \ - a \ - an \ - the - -# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then -# doxygen will generate a detailed section even if there is only a brief -# description. -# The default value is: NO. - -ALWAYS_DETAILED_SEC = NO - -# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all -# inherited members of a class in the documentation of that class as if those -# members were ordinary class members. Constructors, destructors and assignment -# operators of the base classes will not be shown. -# The default value is: NO. - -INLINE_INHERITED_MEMB = NO - -# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path -# before files name in the file list and in the header files. If set to NO the -# shortest path that makes the file name unique will be used -# The default value is: YES. - -FULL_PATH_NAMES = YES - -# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. -# Stripping is only done if one of the specified strings matches the left-hand -# part of the path. The tag can be used to show relative paths in the file list. -# If left blank the directory from which doxygen is run is used as the path to -# strip. -# -# Note that you can specify absolute paths here, but also relative paths, which -# will be relative from the directory where doxygen is started. -# This tag requires that the tag FULL_PATH_NAMES is set to YES. - -STRIP_FROM_PATH = - -# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the -# path mentioned in the documentation of a class, which tells the reader which -# header file to include in order to use a class. If left blank only the name of -# the header file containing the class definition is used. Otherwise one should -# specify the list of include paths that are normally passed to the compiler -# using the -I flag. - -STRIP_FROM_INC_PATH = - -# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but -# less readable) file names. This can be useful is your file systems doesn't -# support long names like on DOS, Mac, or CD-ROM. -# The default value is: NO. - -SHORT_NAMES = NO - -# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the -# first line (until the first dot) of a Javadoc-style comment as the brief -# description. If set to NO, the Javadoc-style will behave just like regular Qt- -# style comments (thus requiring an explicit @brief command for a brief -# description.) -# The default value is: NO. - -JAVADOC_AUTOBRIEF = NO - -# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line -# such as -# /*************** -# as being the beginning of a Javadoc-style comment "banner". If set to NO, the -# Javadoc-style will behave just like regular comments and it will not be -# interpreted by doxygen. -# The default value is: NO. - -JAVADOC_BANNER = NO - -# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first -# line (until the first dot) of a Qt-style comment as the brief description. If -# set to NO, the Qt-style will behave just like regular Qt-style comments (thus -# requiring an explicit \brief command for a brief description.) -# The default value is: NO. - -QT_AUTOBRIEF = NO - -# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a -# multi-line C++ special comment block (i.e. a block of //! or /// comments) as -# a brief description. This used to be the default behavior. The new default is -# to treat a multi-line C++ comment block as a detailed description. Set this -# tag to YES if you prefer the old behavior instead. -# -# Note that setting this tag to YES also means that rational rose comments are -# not recognized any more. -# The default value is: NO. - -MULTILINE_CPP_IS_BRIEF = NO - -# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the -# documentation from any documented member that it re-implements. -# The default value is: YES. - -INHERIT_DOCS = YES - -# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new -# page for each member. If set to NO, the documentation of a member will be part -# of the file/class/namespace that contains it. -# The default value is: NO. - -SEPARATE_MEMBER_PAGES = NO - -# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen -# uses this value to replace tabs by spaces in code fragments. -# Minimum value: 1, maximum value: 16, default value: 4. - -TAB_SIZE = 4 - -# This tag can be used to specify a number of aliases that act as commands in -# the documentation. An alias has the form: -# name=value -# For example adding -# "sideeffect=@par Side Effects:\n" -# will allow you to put the command \sideeffect (or @sideeffect) in the -# documentation, which will result in a user-defined paragraph with heading -# "Side Effects:". You can put \n's in the value part of an alias to insert -# newlines (in the resulting output). You can put ^^ in the value part of an -# alias to insert a newline as if a physical newline was in the original file. -# When you need a literal { or } or , in the value part of an alias you have to -# escape them by means of a backslash (\), this can lead to conflicts with the -# commands \{ and \} for these it is advised to use the version @{ and @} or use -# a double escape (\\{ and \\}) - -ALIASES = - -# This tag can be used to specify a number of word-keyword mappings (TCL only). -# A mapping has the form "name=value". For example adding "class=itcl::class" -# will allow you to use the command class in the itcl::class meaning. - -TCL_SUBST = - -# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources -# only. Doxygen will then generate output that is more tailored for C. For -# instance, some of the names that are used will be different. The list of all -# members will be omitted, etc. -# The default value is: NO. - -OPTIMIZE_OUTPUT_FOR_C = NO - -# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or -# Python sources only. Doxygen will then generate output that is more tailored -# for that language. For instance, namespaces will be presented as packages, -# qualified scopes will look different, etc. -# The default value is: NO. - -OPTIMIZE_OUTPUT_JAVA = NO - -# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran -# sources. Doxygen will then generate output that is tailored for Fortran. -# The default value is: NO. - -OPTIMIZE_FOR_FORTRAN = NO - -# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL -# sources. Doxygen will then generate output that is tailored for VHDL. -# The default value is: NO. - -OPTIMIZE_OUTPUT_VHDL = NO - -# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice -# sources only. Doxygen will then generate output that is more tailored for that -# language. For instance, namespaces will be presented as modules, types will be -# separated into more groups, etc. -# The default value is: NO. - -OPTIMIZE_OUTPUT_SLICE = NO - -# Doxygen selects the parser to use depending on the extension of the files it -# parses. With this tag you can assign which parser to use for a given -# extension. Doxygen has a built-in mapping, but you can override or extend it -# using this tag. The format is ext=language, where ext is a file extension, and -# language is one of the parsers supported by doxygen: IDL, Java, Javascript, -# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice, -# Fortran (fixed format Fortran: FortranFixed, free formatted Fortran: -# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser -# tries to guess whether the code is fixed or free formatted code, this is the -# default for Fortran type files), VHDL, tcl. For instance to make doxygen treat -# .inc files as Fortran files (default is PHP), and .f files as C (default is -# Fortran), use: inc=Fortran f=C. -# -# Note: For files without extension you can use no_extension as a placeholder. -# -# Note that for custom extensions you also need to set FILE_PATTERNS otherwise -# the files are not read by doxygen. - -EXTENSION_MAPPING = - -# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments -# according to the Markdown format, which allows for more readable -# documentation. See https://daringfireball.net/projects/markdown/ for details. -# The output of markdown processing is further processed by doxygen, so you can -# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in -# case of backward compatibilities issues. -# The default value is: YES. - -MARKDOWN_SUPPORT = YES - -# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up -# to that level are automatically included in the table of contents, even if -# they do not have an id attribute. -# Note: This feature currently applies only to Markdown headings. -# Minimum value: 0, maximum value: 99, default value: 5. -# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. - -TOC_INCLUDE_HEADINGS = 0 - -# When enabled doxygen tries to link words that correspond to documented -# classes, or namespaces to their corresponding documentation. Such a link can -# be prevented in individual cases by putting a % sign in front of the word or -# globally by setting AUTOLINK_SUPPORT to NO. -# The default value is: YES. - -AUTOLINK_SUPPORT = YES - -# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want -# to include (a tag file for) the STL sources as input, then you should set this -# tag to YES in order to let doxygen match functions declarations and -# definitions whose arguments contain STL classes (e.g. func(std::string); -# versus func(std::string) {}). This also make the inheritance and collaboration -# diagrams that involve STL classes more complete and accurate. -# The default value is: NO. - -BUILTIN_STL_SUPPORT = NO - -# If you use Microsoft's C++/CLI language, you should set this option to YES to -# enable parsing support. -# The default value is: NO. - -CPP_CLI_SUPPORT = NO - -# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: -# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen -# will parse them like normal C++ but will assume all classes use public instead -# of private inheritance when no explicit protection keyword is present. -# The default value is: NO. - -SIP_SUPPORT = NO - -# For Microsoft's IDL there are propget and propput attributes to indicate -# getter and setter methods for a property. Setting this option to YES will make -# doxygen to replace the get and set methods by a property in the documentation. -# This will only work if the methods are indeed getting or setting a simple -# type. If this is not the case, or you want to show the methods anyway, you -# should set this option to NO. -# The default value is: YES. - -IDL_PROPERTY_SUPPORT = YES - -# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC -# tag is set to YES then doxygen will reuse the documentation of the first -# member in the group (if any) for the other members of the group. By default -# all members of a group must be documented explicitly. -# The default value is: NO. - -DISTRIBUTE_GROUP_DOC = NO - -# If one adds a struct or class to a group and this option is enabled, then also -# any nested class or struct is added to the same group. By default this option -# is disabled and one has to add nested compounds explicitly via \ingroup. -# The default value is: NO. - -GROUP_NESTED_COMPOUNDS = NO - -# Set the SUBGROUPING tag to YES to allow class member groups of the same type -# (for instance a group of public functions) to be put as a subgroup of that -# type (e.g. under the Public Functions section). Set it to NO to prevent -# subgrouping. Alternatively, this can be done per class using the -# \nosubgrouping command. -# The default value is: YES. - -SUBGROUPING = YES - -# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions -# are shown inside the group in which they are included (e.g. using \ingroup) -# instead of on a separate page (for HTML and Man pages) or section (for LaTeX -# and RTF). -# -# Note that this feature does not work in combination with -# SEPARATE_MEMBER_PAGES. -# The default value is: NO. - -INLINE_GROUPED_CLASSES = NO - -# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions -# with only public data fields or simple typedef fields will be shown inline in -# the documentation of the scope in which they are defined (i.e. file, -# namespace, or group documentation), provided this scope is documented. If set -# to NO, structs, classes, and unions are shown on a separate page (for HTML and -# Man pages) or section (for LaTeX and RTF). -# The default value is: NO. - -INLINE_SIMPLE_STRUCTS = NO - -# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or -# enum is documented as struct, union, or enum with the name of the typedef. So -# typedef struct TypeS {} TypeT, will appear in the documentation as a struct -# with name TypeT. When disabled the typedef will appear as a member of a file, -# namespace, or class. And the struct will be named TypeS. This can typically be -# useful for C code in case the coding convention dictates that all compound -# types are typedef'ed and only the typedef is referenced, never the tag name. -# The default value is: NO. - -TYPEDEF_HIDES_STRUCT = NO - -# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This -# cache is used to resolve symbols given their name and scope. Since this can be -# an expensive process and often the same symbol appears multiple times in the -# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small -# doxygen will become slower. If the cache is too large, memory is wasted. The -# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range -# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 -# symbols. At the end of a run doxygen will report the cache usage and suggest -# the optimal cache size from a speed point of view. -# Minimum value: 0, maximum value: 9, default value: 0. - -LOOKUP_CACHE_SIZE = 0 - -#--------------------------------------------------------------------------- -# Build related configuration options -#--------------------------------------------------------------------------- - -# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in -# documentation are documented, even if no documentation was available. Private -# class members and static file members will be hidden unless the -# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. -# Note: This will also disable the warnings about undocumented members that are -# normally produced when WARNINGS is set to YES. -# The default value is: NO. - -EXTRACT_ALL = YES - -# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will -# be included in the documentation. -# The default value is: NO. - -EXTRACT_PRIVATE = NO - -# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual -# methods of a class will be included in the documentation. -# The default value is: NO. - -EXTRACT_PRIV_VIRTUAL = NO - -# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal -# scope will be included in the documentation. -# The default value is: NO. - -EXTRACT_PACKAGE = YES - -# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be -# included in the documentation. -# The default value is: NO. - -EXTRACT_STATIC = YES - -# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined -# locally in source files will be included in the documentation. If set to NO, -# only classes defined in header files are included. Does not have any effect -# for Java sources. -# The default value is: YES. - -EXTRACT_LOCAL_CLASSES = YES - -# This flag is only useful for Objective-C code. If set to YES, local methods, -# which are defined in the implementation section but not in the interface are -# included in the documentation. If set to NO, only methods in the interface are -# included. -# The default value is: NO. - -EXTRACT_LOCAL_METHODS = NO - -# If this flag is set to YES, the members of anonymous namespaces will be -# extracted and appear in the documentation as a namespace called -# 'anonymous_namespace{file}', where file will be replaced with the base name of -# the file that contains the anonymous namespace. By default anonymous namespace -# are hidden. -# The default value is: NO. - -EXTRACT_ANON_NSPACES = NO - -# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all -# undocumented members inside documented classes or files. If set to NO these -# members will be included in the various overviews, but no documentation -# section is generated. This option has no effect if EXTRACT_ALL is enabled. -# The default value is: NO. - -HIDE_UNDOC_MEMBERS = NO - -# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all -# undocumented classes that are normally visible in the class hierarchy. If set -# to NO, these classes will be included in the various overviews. This option -# has no effect if EXTRACT_ALL is enabled. -# The default value is: NO. - -HIDE_UNDOC_CLASSES = NO - -# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend -# (class|struct|union) declarations. If set to NO, these declarations will be -# included in the documentation. -# The default value is: NO. - -HIDE_FRIEND_COMPOUNDS = NO - -# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any -# documentation blocks found inside the body of a function. If set to NO, these -# blocks will be appended to the function's detailed documentation block. -# The default value is: NO. - -HIDE_IN_BODY_DOCS = NO - -# The INTERNAL_DOCS tag determines if documentation that is typed after a -# \internal command is included. If the tag is set to NO then the documentation -# will be excluded. Set it to YES to include the internal documentation. -# The default value is: NO. - -INTERNAL_DOCS = NO - -# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file -# names in lower-case letters. If set to YES, upper-case letters are also -# allowed. This is useful if you have classes or files whose names only differ -# in case and if your file system supports case sensitive file names. Windows -# (including Cygwin) ands Mac users are advised to set this option to NO. -# The default value is: system dependent. - -CASE_SENSE_NAMES = NO - -# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with -# their full class and namespace scopes in the documentation. If set to YES, the -# scope will be hidden. -# The default value is: NO. - -HIDE_SCOPE_NAMES = NO - -# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will -# append additional text to a page's title, such as Class Reference. If set to -# YES the compound reference will be hidden. -# The default value is: NO. - -HIDE_COMPOUND_REFERENCE= NO - -# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of -# the files that are included by a file in the documentation of that file. -# The default value is: YES. - -SHOW_INCLUDE_FILES = YES - -# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each -# grouped member an include statement to the documentation, telling the reader -# which file to include in order to use the member. -# The default value is: NO. - -SHOW_GROUPED_MEMB_INC = NO - -# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include -# files with double quotes in the documentation rather than with sharp brackets. -# The default value is: NO. - -FORCE_LOCAL_INCLUDES = NO - -# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the -# documentation for inline members. -# The default value is: YES. - -INLINE_INFO = YES - -# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the -# (detailed) documentation of file and class members alphabetically by member -# name. If set to NO, the members will appear in declaration order. -# The default value is: YES. - -SORT_MEMBER_DOCS = YES - -# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief -# descriptions of file, namespace and class members alphabetically by member -# name. If set to NO, the members will appear in declaration order. Note that -# this will also influence the order of the classes in the class list. -# The default value is: NO. - -SORT_BRIEF_DOCS = NO - -# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the -# (brief and detailed) documentation of class members so that constructors and -# destructors are listed first. If set to NO the constructors will appear in the -# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. -# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief -# member documentation. -# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting -# detailed member documentation. -# The default value is: NO. - -SORT_MEMBERS_CTORS_1ST = NO - -# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy -# of group names into alphabetical order. If set to NO the group names will -# appear in their defined order. -# The default value is: NO. - -SORT_GROUP_NAMES = NO - -# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by -# fully-qualified names, including namespaces. If set to NO, the class list will -# be sorted only by class name, not including the namespace part. -# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. -# Note: This option applies only to the class list, not to the alphabetical -# list. -# The default value is: NO. - -SORT_BY_SCOPE_NAME = NO - -# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper -# type resolution of all parameters of a function it will reject a match between -# the prototype and the implementation of a member function even if there is -# only one candidate or it is obvious which candidate to choose by doing a -# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still -# accept a match between prototype and implementation in such cases. -# The default value is: NO. - -STRICT_PROTO_MATCHING = NO - -# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo -# list. This list is created by putting \todo commands in the documentation. -# The default value is: YES. - -GENERATE_TODOLIST = YES - -# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test -# list. This list is created by putting \test commands in the documentation. -# The default value is: YES. - -GENERATE_TESTLIST = YES - -# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug -# list. This list is created by putting \bug commands in the documentation. -# The default value is: YES. - -GENERATE_BUGLIST = YES - -# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) -# the deprecated list. This list is created by putting \deprecated commands in -# the documentation. -# The default value is: YES. - -GENERATE_DEPRECATEDLIST= YES - -# The ENABLED_SECTIONS tag can be used to enable conditional documentation -# sections, marked by \if ... \endif and \cond -# ... \endcond blocks. - -ENABLED_SECTIONS = - -# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the -# initial value of a variable or macro / define can have for it to appear in the -# documentation. If the initializer consists of more lines than specified here -# it will be hidden. Use a value of 0 to hide initializers completely. The -# appearance of the value of individual variables and macros / defines can be -# controlled using \showinitializer or \hideinitializer command in the -# documentation regardless of this setting. -# Minimum value: 0, maximum value: 10000, default value: 30. - -MAX_INITIALIZER_LINES = 30 - -# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at -# the bottom of the documentation of classes and structs. If set to YES, the -# list will mention the files that were used to generate the documentation. -# The default value is: YES. - -SHOW_USED_FILES = YES - -# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This -# will remove the Files entry from the Quick Index and from the Folder Tree View -# (if specified). -# The default value is: YES. - -SHOW_FILES = YES - -# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces -# page. This will remove the Namespaces entry from the Quick Index and from the -# Folder Tree View (if specified). -# The default value is: YES. - -SHOW_NAMESPACES = YES - -# The FILE_VERSION_FILTER tag can be used to specify a program or script that -# doxygen should invoke to get the current version for each file (typically from -# the version control system). Doxygen will invoke the program by executing (via -# popen()) the command command input-file, where command is the value of the -# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided -# by doxygen. Whatever the program writes to standard output is used as the file -# version. For an example see the documentation. - -FILE_VERSION_FILTER = - -# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed -# by doxygen. The layout file controls the global structure of the generated -# output files in an output format independent way. To create the layout file -# that represents doxygen's defaults, run doxygen with the -l option. You can -# optionally specify a file name after the option, if omitted DoxygenLayout.xml -# will be used as the name of the layout file. -# -# Note that if you run doxygen from a directory containing a file called -# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE -# tag is left empty. - -LAYOUT_FILE = - -# The CITE_BIB_FILES tag can be used to specify one or more bib files containing -# the reference definitions. This must be a list of .bib files. The .bib -# extension is automatically appended if omitted. This requires the bibtex tool -# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info. -# For LaTeX the style of the bibliography can be controlled using -# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the -# search path. See also \cite for info how to create references. - -CITE_BIB_FILES = - -#--------------------------------------------------------------------------- -# Configuration options related to warning and progress messages -#--------------------------------------------------------------------------- - -# The QUIET tag can be used to turn on/off the messages that are generated to -# standard output by doxygen. If QUIET is set to YES this implies that the -# messages are off. -# The default value is: NO. - -QUIET = NO - -# The WARNINGS tag can be used to turn on/off the warning messages that are -# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES -# this implies that the warnings are on. -# -# Tip: Turn warnings on while writing the documentation. -# The default value is: YES. - -WARNINGS = YES - -# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate -# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag -# will automatically be disabled. -# The default value is: YES. - -WARN_IF_UNDOCUMENTED = YES - -# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for -# potential errors in the documentation, such as not documenting some parameters -# in a documented function, or documenting parameters that don't exist or using -# markup commands wrongly. -# The default value is: YES. - -WARN_IF_DOC_ERROR = YES - -# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that -# are documented, but have no documentation for their parameters or return -# value. If set to NO, doxygen will only warn about wrong or incomplete -# parameter documentation, but not about the absence of documentation. If -# EXTRACT_ALL is set to YES then this flag will automatically be disabled. -# The default value is: NO. - -WARN_NO_PARAMDOC = NO - -# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when -# a warning is encountered. -# The default value is: NO. - -WARN_AS_ERROR = NO - -# The WARN_FORMAT tag determines the format of the warning messages that doxygen -# can produce. The string should contain the $file, $line, and $text tags, which -# will be replaced by the file and line number from which the warning originated -# and the warning text. Optionally the format may contain $version, which will -# be replaced by the version of the file (if it could be obtained via -# FILE_VERSION_FILTER) -# The default value is: $file:$line: $text. - -WARN_FORMAT = "$file:$line: $text" - -# The WARN_LOGFILE tag can be used to specify a file to which warning and error -# messages should be written. If left blank the output is written to standard -# error (stderr). - -WARN_LOGFILE = - -#--------------------------------------------------------------------------- -# Configuration options related to the input files -#--------------------------------------------------------------------------- - -# The INPUT tag is used to specify the files and/or directories that contain -# documented source files. You may enter file names like myfile.cpp or -# directories like /usr/src/myproject. Separate the files or directories with -# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING -# Note: If this tag is empty the current directory is searched. - -INPUT = sdk_core/include \ - README.md - -# This tag can be used to specify the character encoding of the source files -# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses -# libiconv (or the iconv built into libc) for the transcoding. See the libiconv -# documentation (see: https://www.gnu.org/software/libiconv/) for the list of -# possible encodings. -# The default value is: UTF-8. - -INPUT_ENCODING = UTF-8 - -# If the value of the INPUT tag contains directories, you can use the -# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and -# *.h) to filter out the source-files in the directories. -# -# Note that for custom extensions or not directly supported extensions you also -# need to set EXTENSION_MAPPING for the extension otherwise the files are not -# read by doxygen. -# -# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, -# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, -# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, -# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, -# *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf, *.qsf and *.ice. - -FILE_PATTERNS = *.c \ - *.cc \ - *.cxx \ - *.cpp \ - *.c++ \ - *.java \ - *.ii \ - *.ixx \ - *.ipp \ - *.i++ \ - *.inl \ - *.idl \ - *.ddl \ - *.odl \ - *.h \ - *.hh \ - *.hxx \ - *.hpp \ - *.h++ \ - *.cs \ - *.d \ - *.php \ - *.php4 \ - *.php5 \ - *.phtml \ - *.inc \ - *.m \ - *.markdown \ - *.md \ - *.mm \ - *.dox \ - *.py \ - *.pyw \ - *.f90 \ - *.f95 \ - *.f03 \ - *.f08 \ - *.f \ - *.for \ - *.tcl \ - *.vhd \ - *.vhdl \ - *.ucf \ - *.qsf - -# The RECURSIVE tag can be used to specify whether or not subdirectories should -# be searched for input files as well. -# The default value is: NO. - -RECURSIVE = YES - -# The EXCLUDE tag can be used to specify files and/or directories that should be -# excluded from the INPUT source files. This way you can easily exclude a -# subdirectory from a directory tree whose root is specified with the INPUT tag. -# -# Note that relative paths are relative to the directory from which doxygen is -# run. - -EXCLUDE = - -# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or -# directories that are symbolic links (a Unix file system feature) are excluded -# from the input. -# The default value is: NO. - -EXCLUDE_SYMLINKS = NO - -# If the value of the INPUT tag contains directories, you can use the -# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude -# certain files from those directories. -# -# Note that the wildcards are matched against the file with absolute path, so to -# exclude all test directories for example use the pattern */test/* - -EXCLUDE_PATTERNS = - -# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names -# (namespaces, classes, functions, etc.) that should be excluded from the -# output. The symbol name can be a fully qualified name, a word, or if the -# wildcard * is used, a substring. Examples: ANamespace, AClass, -# AClass::ANamespace, ANamespace::*Test -# -# Note that the wildcards are matched against the file with absolute path, so to -# exclude all test directories use the pattern */test/* - -EXCLUDE_SYMBOLS = - -# The EXAMPLE_PATH tag can be used to specify one or more files or directories -# that contain example code fragments that are included (see the \include -# command). - -EXAMPLE_PATH = - -# If the value of the EXAMPLE_PATH tag contains directories, you can use the -# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and -# *.h) to filter out the source-files in the directories. If left blank all -# files are included. - -EXAMPLE_PATTERNS = * - -# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be -# searched for input files to be used with the \include or \dontinclude commands -# irrespective of the value of the RECURSIVE tag. -# The default value is: NO. - -EXAMPLE_RECURSIVE = NO - -# The IMAGE_PATH tag can be used to specify one or more files or directories -# that contain images that are to be included in the documentation (see the -# \image command). - -IMAGE_PATH = . - -# The INPUT_FILTER tag can be used to specify a program that doxygen should -# invoke to filter for each input file. Doxygen will invoke the filter program -# by executing (via popen()) the command: -# -# -# -# where is the value of the INPUT_FILTER tag, and is the -# name of an input file. Doxygen will then use the output that the filter -# program writes to standard output. If FILTER_PATTERNS is specified, this tag -# will be ignored. -# -# Note that the filter must not add or remove lines; it is applied before the -# code is scanned, but not when the output code is generated. If lines are added -# or removed, the anchors will not be placed correctly. -# -# Note that for custom extensions or not directly supported extensions you also -# need to set EXTENSION_MAPPING for the extension otherwise the files are not -# properly processed by doxygen. - -INPUT_FILTER = - -# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern -# basis. Doxygen will compare the file name with each pattern and apply the -# filter if there is a match. The filters are a list of the form: pattern=filter -# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how -# filters are used. If the FILTER_PATTERNS tag is empty or if none of the -# patterns match the file name, INPUT_FILTER is applied. -# -# Note that for custom extensions or not directly supported extensions you also -# need to set EXTENSION_MAPPING for the extension otherwise the files are not -# properly processed by doxygen. - -FILTER_PATTERNS = - -# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using -# INPUT_FILTER) will also be used to filter the input files that are used for -# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). -# The default value is: NO. - -FILTER_SOURCE_FILES = NO - -# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file -# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and -# it is also possible to disable source filtering for a specific pattern using -# *.ext= (so without naming a filter). -# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. - -FILTER_SOURCE_PATTERNS = - -# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that -# is part of the input, its contents will be placed on the main page -# (index.html). This can be useful if you have a project on for instance GitHub -# and want to reuse the introduction page also for the doxygen output. - -USE_MDFILE_AS_MAINPAGE = README.md - -#--------------------------------------------------------------------------- -# Configuration options related to source browsing -#--------------------------------------------------------------------------- - -# If the SOURCE_BROWSER tag is set to YES then a list of source files will be -# generated. Documented entities will be cross-referenced with these sources. -# -# Note: To get rid of all source code in the generated output, make sure that -# also VERBATIM_HEADERS is set to NO. -# The default value is: NO. - -SOURCE_BROWSER = YES - -# Setting the INLINE_SOURCES tag to YES will include the body of functions, -# classes and enums directly into the documentation. -# The default value is: NO. - -INLINE_SOURCES = NO - -# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any -# special comment blocks from generated source code fragments. Normal C, C++ and -# Fortran comments will always remain visible. -# The default value is: YES. - -STRIP_CODE_COMMENTS = YES - -# If the REFERENCED_BY_RELATION tag is set to YES then for each documented -# entity all documented functions referencing it will be listed. -# The default value is: NO. - -REFERENCED_BY_RELATION = NO - -# If the REFERENCES_RELATION tag is set to YES then for each documented function -# all documented entities called/used by that function will be listed. -# The default value is: NO. - -REFERENCES_RELATION = NO - -# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set -# to YES then the hyperlinks from functions in REFERENCES_RELATION and -# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will -# link to the documentation. -# The default value is: YES. - -REFERENCES_LINK_SOURCE = YES - -# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the -# source code will show a tooltip with additional information such as prototype, -# brief description and links to the definition and documentation. Since this -# will make the HTML file larger and loading of large files a bit slower, you -# can opt to disable this feature. -# The default value is: YES. -# This tag requires that the tag SOURCE_BROWSER is set to YES. - -SOURCE_TOOLTIPS = YES - -# If the USE_HTAGS tag is set to YES then the references to source code will -# point to the HTML generated by the htags(1) tool instead of doxygen built-in -# source browser. The htags tool is part of GNU's global source tagging system -# (see https://www.gnu.org/software/global/global.html). You will need version -# 4.8.6 or higher. -# -# To use it do the following: -# - Install the latest version of global -# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file -# - Make sure the INPUT points to the root of the source tree -# - Run doxygen as normal -# -# Doxygen will invoke htags (and that will in turn invoke gtags), so these -# tools must be available from the command line (i.e. in the search path). -# -# The result: instead of the source browser generated by doxygen, the links to -# source code will now point to the output of htags. -# The default value is: NO. -# This tag requires that the tag SOURCE_BROWSER is set to YES. - -USE_HTAGS = NO - -# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a -# verbatim copy of the header file for each class for which an include is -# specified. Set to NO to disable this. -# See also: Section \class. -# The default value is: YES. - -VERBATIM_HEADERS = YES - -# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the -# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the -# cost of reduced performance. This can be particularly helpful with template -# rich C++ code for which doxygen's built-in parser lacks the necessary type -# information. -# Note: The availability of this option depends on whether or not doxygen was -# generated with the -Duse_libclang=ON option for CMake. -# The default value is: NO. - -CLANG_ASSISTED_PARSING = NO - -# If clang assisted parsing is enabled you can provide the compiler with command -# line options that you would normally use when invoking the compiler. Note that -# the include paths will already be set by doxygen for the files and directories -# specified with INPUT and INCLUDE_PATH. -# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES. - -CLANG_OPTIONS = - -# If clang assisted parsing is enabled you can provide the clang parser with the -# path to the compilation database (see: -# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) used when the files -# were built. This is equivalent to specifying the "-p" option to a clang tool, -# such as clang-check. These options will then be passed to the parser. -# Note: The availability of this option depends on whether or not doxygen was -# generated with the -Duse_libclang=ON option for CMake. - -CLANG_DATABASE_PATH = - -#--------------------------------------------------------------------------- -# Configuration options related to the alphabetical class index -#--------------------------------------------------------------------------- - -# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all -# compounds will be generated. Enable this if the project contains a lot of -# classes, structs, unions or interfaces. -# The default value is: YES. - -ALPHABETICAL_INDEX = YES - -# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in -# which the alphabetical index list will be split. -# Minimum value: 1, maximum value: 20, default value: 5. -# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. - -COLS_IN_ALPHA_INDEX = 5 - -# In case all classes in a project start with a common prefix, all classes will -# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag -# can be used to specify a prefix (or a list of prefixes) that should be ignored -# while generating the index headers. -# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. - -IGNORE_PREFIX = - -#--------------------------------------------------------------------------- -# Configuration options related to the HTML output -#--------------------------------------------------------------------------- - -# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output -# The default value is: YES. - -GENERATE_HTML = YES - -# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a -# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of -# it. -# The default directory is: html. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_OUTPUT = html - -# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each -# generated HTML page (for example: .htm, .php, .asp). -# The default value is: .html. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_FILE_EXTENSION = .html - -# The HTML_HEADER tag can be used to specify a user-defined HTML header file for -# each generated HTML page. If the tag is left blank doxygen will generate a -# standard header. -# -# To get valid HTML the header file that includes any scripts and style sheets -# that doxygen needs, which is dependent on the configuration options used (e.g. -# the setting GENERATE_TREEVIEW). It is highly recommended to start with a -# default header using -# doxygen -w html new_header.html new_footer.html new_stylesheet.css -# YourConfigFile -# and then modify the file new_header.html. See also section "Doxygen usage" -# for information on how to generate the default header that doxygen normally -# uses. -# Note: The header is subject to change so you typically have to regenerate the -# default header when upgrading to a newer version of doxygen. For a description -# of the possible markers and block names see the documentation. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_HEADER = - -# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each -# generated HTML page. If the tag is left blank doxygen will generate a standard -# footer. See HTML_HEADER for more information on how to generate a default -# footer and what special commands can be used inside the footer. See also -# section "Doxygen usage" for information on how to generate the default footer -# that doxygen normally uses. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_FOOTER = - -# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style -# sheet that is used by each HTML page. It can be used to fine-tune the look of -# the HTML output. If left blank doxygen will generate a default style sheet. -# See also section "Doxygen usage" for information on how to generate the style -# sheet that doxygen normally uses. -# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as -# it is more robust and this tag (HTML_STYLESHEET) will in the future become -# obsolete. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_STYLESHEET = - -# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined -# cascading style sheets that are included after the standard style sheets -# created by doxygen. Using this option one can overrule certain style aspects. -# This is preferred over using HTML_STYLESHEET since it does not replace the -# standard style sheet and is therefore more robust against future updates. -# Doxygen will copy the style sheet files to the output directory. -# Note: The order of the extra style sheet files is of importance (e.g. the last -# style sheet in the list overrules the setting of the previous ones in the -# list). For an example see the documentation. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_EXTRA_STYLESHEET = - -# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or -# other source files which should be copied to the HTML output directory. Note -# that these files will be copied to the base HTML output directory. Use the -# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these -# files. In the HTML_STYLESHEET file, use the file name only. Also note that the -# files will be copied as-is; there are no commands or markers available. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_EXTRA_FILES = - -# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen -# will adjust the colors in the style sheet and background images according to -# this color. Hue is specified as an angle on a colorwheel, see -# https://en.wikipedia.org/wiki/Hue for more information. For instance the value -# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 -# purple, and 360 is red again. -# Minimum value: 0, maximum value: 359, default value: 220. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_COLORSTYLE_HUE = 220 - -# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors -# in the HTML output. For a value of 0 the output will use grayscales only. A -# value of 255 will produce the most vivid colors. -# Minimum value: 0, maximum value: 255, default value: 100. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_COLORSTYLE_SAT = 100 - -# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the -# luminance component of the colors in the HTML output. Values below 100 -# gradually make the output lighter, whereas values above 100 make the output -# darker. The value divided by 100 is the actual gamma applied, so 80 represents -# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not -# change the gamma. -# Minimum value: 40, maximum value: 240, default value: 80. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_COLORSTYLE_GAMMA = 80 - -# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML -# page will contain the date and time when the page was generated. Setting this -# to YES can help to show when doxygen was last run and thus if the -# documentation is up to date. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_TIMESTAMP = NO - -# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML -# documentation will contain a main index with vertical navigation menus that -# are dynamically created via Javascript. If disabled, the navigation index will -# consists of multiple levels of tabs that are statically embedded in every HTML -# page. Disable this option to support browsers that do not have Javascript, -# like the Qt help browser. -# The default value is: YES. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_DYNAMIC_MENUS = YES - -# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML -# documentation will contain sections that can be hidden and shown after the -# page has loaded. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_DYNAMIC_SECTIONS = NO - -# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries -# shown in the various tree structured indices initially; the user can expand -# and collapse entries dynamically later on. Doxygen will expand the tree to -# such a level that at most the specified number of entries are visible (unless -# a fully collapsed tree already exceeds this amount). So setting the number of -# entries 1 will produce a full collapsed tree by default. 0 is a special value -# representing an infinite number of entries and will result in a full expanded -# tree by default. -# Minimum value: 0, maximum value: 9999, default value: 100. -# This tag requires that the tag GENERATE_HTML is set to YES. - -HTML_INDEX_NUM_ENTRIES = 100 - -# If the GENERATE_DOCSET tag is set to YES, additional index files will be -# generated that can be used as input for Apple's Xcode 3 integrated development -# environment (see: https://developer.apple.com/xcode/), introduced with OSX -# 10.5 (Leopard). To create a documentation set, doxygen will generate a -# Makefile in the HTML output directory. Running make will produce the docset in -# that directory and running make install will install the docset in -# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at -# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy -# genXcode/_index.html for more information. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_DOCSET = NO - -# This tag determines the name of the docset feed. A documentation feed provides -# an umbrella under which multiple documentation sets from a single provider -# (such as a company or product suite) can be grouped. -# The default value is: Doxygen generated docs. -# This tag requires that the tag GENERATE_DOCSET is set to YES. - -DOCSET_FEEDNAME = "Doxygen generated docs" - -# This tag specifies a string that should uniquely identify the documentation -# set bundle. This should be a reverse domain-name style string, e.g. -# com.mycompany.MyDocSet. Doxygen will append .docset to the name. -# The default value is: org.doxygen.Project. -# This tag requires that the tag GENERATE_DOCSET is set to YES. - -DOCSET_BUNDLE_ID = org.doxygen.Project - -# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify -# the documentation publisher. This should be a reverse domain-name style -# string, e.g. com.mycompany.MyDocSet.documentation. -# The default value is: org.doxygen.Publisher. -# This tag requires that the tag GENERATE_DOCSET is set to YES. - -DOCSET_PUBLISHER_ID = org.doxygen.Publisher - -# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. -# The default value is: Publisher. -# This tag requires that the tag GENERATE_DOCSET is set to YES. - -DOCSET_PUBLISHER_NAME = Publisher - -# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three -# additional HTML index files: index.hhp, index.hhc, and index.hhk. The -# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop -# (see: https://www.microsoft.com/en-us/download/details.aspx?id=21138) on -# Windows. -# -# The HTML Help Workshop contains a compiler that can convert all HTML output -# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML -# files are now used as the Windows 98 help format, and will replace the old -# Windows help format (.hlp) on all Windows platforms in the future. Compressed -# HTML files also contain an index, a table of contents, and you can search for -# words in the documentation. The HTML workshop also contains a viewer for -# compressed HTML files. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_HTMLHELP = NO - -# The CHM_FILE tag can be used to specify the file name of the resulting .chm -# file. You can add a path in front of the file if the result should not be -# written to the html output directory. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -CHM_FILE = - -# The HHC_LOCATION tag can be used to specify the location (absolute path -# including file name) of the HTML help compiler (hhc.exe). If non-empty, -# doxygen will try to run the HTML help compiler on the generated index.hhp. -# The file has to be specified with full path. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -HHC_LOCATION = - -# The GENERATE_CHI flag controls if a separate .chi index file is generated -# (YES) or that it should be included in the master .chm file (NO). -# The default value is: NO. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -GENERATE_CHI = NO - -# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) -# and project file content. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -CHM_INDEX_ENCODING = - -# The BINARY_TOC flag controls whether a binary table of contents is generated -# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it -# enables the Previous and Next buttons. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -BINARY_TOC = NO - -# The TOC_EXPAND flag can be set to YES to add extra items for group members to -# the table of contents of the HTML help documentation and to the tree view. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTMLHELP is set to YES. - -TOC_EXPAND = NO - -# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and -# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that -# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help -# (.qch) of the generated HTML documentation. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_QHP = NO - -# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify -# the file name of the resulting .qch file. The path specified is relative to -# the HTML output folder. -# This tag requires that the tag GENERATE_QHP is set to YES. - -QCH_FILE = - -# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help -# Project output. For more information please see Qt Help Project / Namespace -# (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace). -# The default value is: org.doxygen.Project. -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_NAMESPACE = org.doxygen.Project - -# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt -# Help Project output. For more information please see Qt Help Project / Virtual -# Folders (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual- -# folders). -# The default value is: doc. -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_VIRTUAL_FOLDER = doc - -# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom -# filter to add. For more information please see Qt Help Project / Custom -# Filters (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom- -# filters). -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_CUST_FILTER_NAME = - -# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the -# custom filter to add. For more information please see Qt Help Project / Custom -# Filters (see: https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom- -# filters). -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_CUST_FILTER_ATTRS = - -# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this -# project's filter section matches. Qt Help Project / Filter Attributes (see: -# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes). -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHP_SECT_FILTER_ATTRS = - -# The QHG_LOCATION tag can be used to specify the location of Qt's -# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the -# generated .qhp file. -# This tag requires that the tag GENERATE_QHP is set to YES. - -QHG_LOCATION = - -# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be -# generated, together with the HTML files, they form an Eclipse help plugin. To -# install this plugin and make it available under the help contents menu in -# Eclipse, the contents of the directory containing the HTML and XML files needs -# to be copied into the plugins directory of eclipse. The name of the directory -# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. -# After copying Eclipse needs to be restarted before the help appears. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_ECLIPSEHELP = NO - -# A unique identifier for the Eclipse help plugin. When installing the plugin -# the directory name containing the HTML and XML files should also have this -# name. Each documentation set should have its own identifier. -# The default value is: org.doxygen.Project. -# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. - -ECLIPSE_DOC_ID = org.doxygen.Project - -# If you want full control over the layout of the generated HTML pages it might -# be necessary to disable the index and replace it with your own. The -# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top -# of each HTML page. A value of NO enables the index and the value YES disables -# it. Since the tabs in the index contain the same information as the navigation -# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -DISABLE_INDEX = NO - -# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index -# structure should be generated to display hierarchical information. If the tag -# value is set to YES, a side panel will be generated containing a tree-like -# index structure (just like the one that is generated for HTML Help). For this -# to work a browser that supports JavaScript, DHTML, CSS and frames is required -# (i.e. any modern browser). Windows users are probably better off using the -# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can -# further fine-tune the look of the index. As an example, the default style -# sheet generated by doxygen has an example that shows how to put an image at -# the root of the tree instead of the PROJECT_NAME. Since the tree basically has -# the same information as the tab index, you could consider setting -# DISABLE_INDEX to YES when enabling this option. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -GENERATE_TREEVIEW = YES - -# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that -# doxygen will group on one line in the generated HTML documentation. -# -# Note that a value of 0 will completely suppress the enum values from appearing -# in the overview section. -# Minimum value: 0, maximum value: 20, default value: 4. -# This tag requires that the tag GENERATE_HTML is set to YES. - -ENUM_VALUES_PER_LINE = 4 - -# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used -# to set the initial width (in pixels) of the frame in which the tree is shown. -# Minimum value: 0, maximum value: 1500, default value: 250. -# This tag requires that the tag GENERATE_HTML is set to YES. - -TREEVIEW_WIDTH = 250 - -# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to -# external symbols imported via tag files in a separate window. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -EXT_LINKS_IN_WINDOW = NO - -# Use this tag to change the font size of LaTeX formulas included as images in -# the HTML documentation. When you change the font size after a successful -# doxygen run you need to manually remove any form_*.png images from the HTML -# output directory to force them to be regenerated. -# Minimum value: 8, maximum value: 50, default value: 10. -# This tag requires that the tag GENERATE_HTML is set to YES. - -FORMULA_FONTSIZE = 10 - -# Use the FORMULA_TRANSPARENT tag to determine whether or not the images -# generated for formulas are transparent PNGs. Transparent PNGs are not -# supported properly for IE 6.0, but are supported on all modern browsers. -# -# Note that when changing this option you need to delete any form_*.png files in -# the HTML output directory before the changes have effect. -# The default value is: YES. -# This tag requires that the tag GENERATE_HTML is set to YES. - -FORMULA_TRANSPARENT = YES - -# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see -# https://www.mathjax.org) which uses client side Javascript for the rendering -# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX -# installed or if you want to formulas look prettier in the HTML output. When -# enabled you may also need to install MathJax separately and configure the path -# to it using the MATHJAX_RELPATH option. -# The default value is: NO. -# This tag requires that the tag GENERATE_HTML is set to YES. - -USE_MATHJAX = NO - -# When MathJax is enabled you can set the default output format to be used for -# the MathJax output. See the MathJax site (see: -# http://docs.mathjax.org/en/latest/output.html) for more details. -# Possible values are: HTML-CSS (which is slower, but has the best -# compatibility), NativeMML (i.e. MathML) and SVG. -# The default value is: HTML-CSS. -# This tag requires that the tag USE_MATHJAX is set to YES. - -MATHJAX_FORMAT = HTML-CSS - -# When MathJax is enabled you need to specify the location relative to the HTML -# output directory using the MATHJAX_RELPATH option. The destination directory -# should contain the MathJax.js script. For instance, if the mathjax directory -# is located at the same level as the HTML output directory, then -# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax -# Content Delivery Network so you can quickly see the result without installing -# MathJax. However, it is strongly recommended to install a local copy of -# MathJax from https://www.mathjax.org before deployment. -# The default value is: https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/. -# This tag requires that the tag USE_MATHJAX is set to YES. - -MATHJAX_RELPATH = https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.2/ - -# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax -# extension names that should be enabled during MathJax rendering. For example -# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols -# This tag requires that the tag USE_MATHJAX is set to YES. - -MATHJAX_EXTENSIONS = - -# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces -# of code that will be used on startup of the MathJax code. See the MathJax site -# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an -# example see the documentation. -# This tag requires that the tag USE_MATHJAX is set to YES. - -MATHJAX_CODEFILE = - -# When the SEARCHENGINE tag is enabled doxygen will generate a search box for -# the HTML output. The underlying search engine uses javascript and DHTML and -# should work on any modern browser. Note that when using HTML help -# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) -# there is already a search function so this one should typically be disabled. -# For large projects the javascript based search engine can be slow, then -# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to -# search using the keyboard; to jump to the search box use + S -# (what the is depends on the OS and browser, but it is typically -# , /