azmo/Livox_SDK2
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit: Livox SDK2 source (excluding build artifacts)

Browse Source
This commit is contained in:
gardentech
2026-08-26 01:38:14 +09:00
commit 34a9a52cd4
221 changed files with 54102 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sdk_core/CMakeLists.txt
+144
View File
@@ -0,0 +1,144 @@
cmake_minimum_required(VERSION 3.0)
set(SDK_LIBRARY_STATIC livox_lidar_sdk_static)
set(SDK_LIBRARY_SHARED livox_lidar_sdk_shared)
add_library(${SDK_LIBRARY_STATIC} STATIC "")
add_library(${SDK_LIBRARY_SHARED} SHARED "")
set(LIVOX_SDK_MAJOR_VERSION "0")
set(LIVOX_SDK_MINOR_VERSION "0")
set(LIVOX_SDK_PATCH_VERSION "2")
set(LIVOX_SDK_VERSION_STRING "${LIVOX_SDK_MAJOR_VERSION}.${LIVOX_SDK_MINOR_VERSION}.${LIVOX_SDK_PATCH_VERSION}")
set(LIVOX_API_HEADER
../include/livox_lidar_def.h
../include/livox_lidar_api.h
../include/livox_lidar_cfg.h
)
set_target_properties(${SDK_LIBRARY_STATIC} #${SDK_LIBRARY_SHARED}
PROPERTIES
PUBLIC_HEADER "${LIVOX_API_HEADER}"
)
if(WIN32)
set(PLATFORM win)
else(WIN32)
set(PLATFORM unix)
endif (WIN32)
target_compile_options(${SDK_LIBRARY_STATIC}
PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>#-Wno-c++11-long-long>
PRIVATE $<$<CXX_COMPILER_ID:AppleClang>:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long>
PRIVATE $<$<CXX_COMPILER_ID:Clang>:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long>
)
target_compile_options(${SDK_LIBRARY_SHARED}
PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>#-Wno-c++11-long-long>
PRIVATE $<$<CXX_COMPILER_ID:AppleClang>:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long>
PRIVATE $<$<CXX_COMPILER_ID:Clang>:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long>
)
set(LIVOX_PRIVATE_INCLUDE_DIR
../3rdparty
../3rdparty/spdlog
.
)
set(LIVOX_PUBLIC_INCLUDE_DIR
../include
)
target_include_directories(
${SDK_LIBRARY_STATIC}
PUBLIC
${LIVOX_PUBLIC_INCLUDE_DIR}
PRIVATE
${LIVOX_PRIVATE_INCLUDE_DIR}
)
target_include_directories(
${SDK_LIBRARY_SHARED}
PUBLIC
${LIVOX_PUBLIC_INCLUDE_DIR}
PRIVATE
${LIVOX_PRIVATE_INCLUDE_DIR}
)
set(MAIN_SOURCES
device_manager.cpp
livox_lidar_sdk.cpp
params_check.cpp
parse_cfg_file.cpp
upgrade_manager.cpp
)
set(BASE_SOURCES
base/io_loop.cpp
base/thread_base.cpp
base/io_thread.cpp
base/logging.cpp
base/network/${PLATFORM}/network_util.cpp
base/multiple_io/multiple_io_base.cpp
base/multiple_io/multiple_io_epoll.cpp
base/multiple_io/multiple_io_poll.cpp
base/multiple_io/multiple_io_select.cpp
base/multiple_io/multiple_io_kqueue.cpp
base/wake_up/${PLATFORM}/wake_up_pipe.cpp
)
set(COMM_SOURCES
comm/comm_port.cpp
comm/sdk_protocol.cpp
comm/generate_seq.cpp
)
set(UPGRADE_SOURCES
upgrade_manager.cpp
upgrade/firmware.cpp
upgrade/livox_lidar_upgrader.cpp
)
set(LOGGER_HANDLER_SOURCES
logger_handler/logger_manager.cpp
logger_handler/logger_handler.cpp
logger_handler/file_manager.cpp
)
set(DATA_HANDLER_SOURCES
data_handler/data_handler.cpp
)
set(COMMAND_HANDLER_SOURCES
command_handler/command_impl.cpp
command_handler/general_command_handler.cpp
command_handler/hap_command_handler.cpp
command_handler/mid360_command_handler.cpp
command_handler/build_request.cpp
command_handler/parse_lidar_state_info.cpp
command_handler/mid360s_command_handler.cpp
)
set(DEBUG_POINT_CLOUD_HANDLER_SOURCES
debug_point_cloud_handler/debug_point_cloud_manager.cpp
debug_point_cloud_handler/debug_point_cloud_handler.cpp
)
set(LIVOX_SOURCES
../3rdparty/FastCRC/FastCRC_tables.h
../3rdparty/FastCRC/FastCRCsw.cpp
${MAIN_SOURCES}
${BASE_SOURCES}
${COMM_SOURCES}
${UPGRADE_SOURCES}
${LOGGER_HANDLER_SOURCES}
${DATA_HANDLER_SOURCES}
${COMMAND_HANDLER_SOURCES}
${DEBUG_POINT_CLOUD_HANDLER_SOURCES}
)
target_sources(${SDK_LIBRARY_STATIC}
PRIVATE
${LIVOX_SOURCES}
)
target_sources(${SDK_LIBRARY_SHARED}
PRIVATE
${LIVOX_SOURCES}
)
install(TARGETS ${SDK_LIBRARY_STATIC} ${SDK_LIBRARY_SHARED}
PUBLIC_HEADER DESTINATION include
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)
sdk_core/base/command_callback.h
+249
View File
@@ -0,0 +1,249 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_COMMAND_CALLBACK_H
#define LIVOX_COMMAND_CALLBACK_H
#include <functional>
#include <memory>
#include "livox_lidar_api.h"
namespace livox {
namespace lidar {
class CommandCallback {
public:
virtual ~CommandCallback() {}
virtual void operator()(livox_status status,uint32_t handle, void *data) = 0;
};
template <class T, class ResponseType>
class MemberFunctionCallback : public CommandCallback {
public:
typedef void (T::*MemFn)(livox_status status, uint32_t handle, ResponseType *data);
MemberFunctionCallback(T *cls, MemFn func) : this_(cls), func_(func) {}
void operator()(livox_status status, uint32_t handle, void *data) {
if (this_) {
(this_->*func_)(status, handle, static_cast<ResponseType *>(data));
}
}
private:
T *this_;
MemFn func_;
};
template <class T>
class MemberFunctionCallback<T, uint8_t> : public CommandCallback {
public:
typedef void (T::*MemFn)(livox_status status, uint32_t handle, uint8_t response);
MemberFunctionCallback(T *cls, MemFn func) : this_(cls), func_(func) {}
void operator()(livox_status status, uint32_t handle, void *data) {
if (this_) {
if (data == NULL) {
(this_->*func_)(status, handle, 0);
} else {
(this_->*func_)(status, handle, static_cast<uint8_t>(reinterpret_cast<uintptr_t>(data)));
}
}
}
private:
T *this_;
MemFn func_;
};
template <class T, class ResponseType>
std::shared_ptr<CommandCallback> MakeCommandCallback(T *cls,
typename MemberFunctionCallback<T, ResponseType>::MemFn func) {
std::shared_ptr<CommandCallback> cb(new MemberFunctionCallback<T, ResponseType>(cls, func));
return cb;
}
template <class T>
class FunctionStatusCallback : public CommandCallback {
public:
typedef void (*Fn)(livox_status status, uint32_t handle, T *data, void *client_data);
public:
FunctionStatusCallback(Fn func, void *client_data) : func_(func), client_data_(client_data) {}
void operator()(livox_status status, uint32_t handle, void *data) {
if (func_) {
(*func_)(status, handle, static_cast<T *>(data), client_data_);
}
}
private:
Fn func_;
void *client_data_;
};
template <>
class FunctionStatusCallback<uint8_t> : public CommandCallback {
public:
typedef void (*Fn)(livox_status status, uint32_t handle, uint8_t data, void *client_data);
public:
FunctionStatusCallback(Fn func, void *client_data) : func_(func), client_data_(client_data) {}
void operator()(livox_status status, uint32_t handle, void *data) {
if (func_) {
if (data == NULL) {
(*func_)(status, handle, 0, client_data_);
} else {
(*func_)(status, handle, *(uint8_t *)data, client_data_);
}
}
}
private:
Fn func_;
void *client_data_;
};
template <class T>
std::shared_ptr<CommandCallback> MakeCommandCallback(typename FunctionStatusCallback<T>::Fn func, void *client_data) {
std::shared_ptr<CommandCallback> cb(new FunctionStatusCallback<T>(func, client_data));
return cb;
}
template <class T>
class MessageCallback : public CommandCallback {
public:
typedef void (*Fn)(livox_status status, uint32_t handle, T *data);
public:
MessageCallback(const Fn &func) : func_(func) {}
void operator()(livox_status status, uint32_t handle, void *data) {
if (func_) {
(*func_)(status, handle, static_cast<T *>(data));
}
}
private:
Fn func_;
};
template <class ResponseType>
class BoostFunctionMessageCallback : public CommandCallback {
public:
typedef std::function<void(livox_status, uint8_t, ResponseType *)> Fn;
public:
BoostFunctionMessageCallback(const Fn &func) : func_(func) {}
void operator()(livox_status status, uint32_t handle, void *data) {
if (func_) {
func_(status, handle, static_cast<ResponseType *>(data));
}
}
private:
Fn func_;
};
template <class T>
std::shared_ptr<CommandCallback> MakeMessageCallback(typename MessageCallback<T>::Fn func) {
std::shared_ptr<CommandCallback> cb(new MessageCallback<T>(func));
return cb;
}
template <class T>
std::shared_ptr<CommandCallback> MakeMessageCallback(typename BoostFunctionMessageCallback<T>::Fn func) {
std::shared_ptr<CommandCallback> cb(new BoostFunctionMessageCallback<T>(func));
return cb;
}
template <class T, class ResponseType>
class MemberMessageCallback : public CommandCallback {
public:
typedef void (T::*MemFn)(livox_status status, uint32_t handle, ResponseType *data);
MemberMessageCallback(T *cls, MemFn func) : this_(cls), func_(func) {}
void operator()(livox_status status,uint32_t handle, void *data) {
if (this_) {
(this_->*func_)(status, handle, (ResponseType *)data);
}
}
private:
T *this_;
MemFn func_;
};
template <class T, class ResponseType>
std::shared_ptr<CommandCallback> MakeMemberMessageCallback(
T *_this,
typename MemberMessageCallback<T, ResponseType>::MemFn func) {
std::shared_ptr<CommandCallback> cb(new MemberMessageCallback<T, ResponseType>(_this, func));
return cb;
}
template <class ResponseType>
class BoostFunctionCallback : public CommandCallback {
public:
typedef std::function<void(uint8_t, uint8_t, ResponseType *)> Fn;
BoostFunctionCallback(const Fn &func) : func_(func) {}
void operator()(livox_status status,uint32_t handle, void *data) {
if (func_) {
if (data == NULL) {
func_(status, handle, NULL);
} else {
func_(status, handle, (ResponseType *)data);
}
}
}
private:
Fn func_;
};
template <>
class BoostFunctionCallback<uint8_t> : public CommandCallback {
public:
typedef std::function<void(uint8_t, uint8_t, uint8_t)> Fn;
BoostFunctionCallback(const Fn &func) : func_(func) {}
void operator()(livox_status status,uint32_t handle, void *data) {
if (func_) {
if (data == NULL) {
func_(status, handle, 0);
} else {
func_(status, handle, static_cast<uint8_t>(reinterpret_cast<uintptr_t>(data)));
}
}
}
private:
Fn func_;
};
template <class ResponseType>
std::shared_ptr<CommandCallback> MakeCommandCallback(const typename BoostFunctionCallback<ResponseType>::Fn &func) {
std::shared_ptr<CommandCallback> cb(new BoostFunctionCallback<ResponseType>(func));
return cb;
}
} // namespace lidar
} // namespace livox
#endif // LIVOX_COMMAND_CALLBACK_H
sdk_core/base/io_loop.cpp
+131
View File
@@ -0,0 +1,131 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "io_loop.h"
#include <functional>
#include <mutex>
#include <iostream>
#include <algorithm>
#include "logging.h"
using std::lock_guard;
using std::mutex;
using std::vector;
using std::chrono::steady_clock;
namespace livox {
namespace lidar {
bool IOLoop::Init() {
auto multiple_io = MultipleIOFactory::CreateMultipleIO();
if (!multiple_io) {
LOG_ERROR("Creat Multiple IO Failed!");
return false;
}
multiple_io_base_ = std::move(multiple_io);
if (!multiple_io_base_->PollCreate(OPEN_MAX_POLL)) {
LOG_ERROR("Poll Create Failed!");
return false;
}
return true;
}
void IOLoop::Uninit() {
multiple_io_base_->PollDestroy();
}
void IOLoop::AddDelegate(socket_t sock, IOLoop::IOLoopDelegate *delegate, void *data) {
PostTask(std::bind(&IOLoop::AddDelegateAsync, this, sock, delegate, data));
}
void IOLoop::RemoveDelegate(socket_t sock, IOLoopDelegate *) {
PostTask(std::bind(&IOLoop::RemoveDelegateAsync, this, sock));
}
void IOLoop::Loop() {
multiple_io_base_->Poll(POLL_TIMEOUT);
vector<IOLoopTask> tasks;
{
lock_guard<mutex> lock(mutex_);
tasks.swap(pending_tasks_);
}
for (auto &task : tasks) {
task();
}
}
bool IOLoop::Wakeup() {
if (multiple_io_base_) {
multiple_io_base_->PollWakeUp();
}
return true;
}
void IOLoop::PostTask(const IOLoopTask &task) {
{
lock_guard<mutex> lock(mutex_);
pending_tasks_.push_back(task);
}
Wakeup();
}
void IOLoop::AddDelegateAsync(socket_t sock, IOLoop::IOLoopDelegate *delegate, void *data) {
PollFd pollfd = {};
pollfd.fd = sock;
pollfd.event = READBLE_EVENT;
pollfd.event_callback = [=](FdEvent event) {
if (event & READBLE_EVENT) {
if (delegate) {
delegate->OnData(sock, data);
}
}
};
if (enable_timer_) {
pollfd.timer_callback = [=](TimePoint t) {
if (delegate) {
delegate->OnTimer(t);
}
};
}
if (enable_wake_) {
pollfd.wake_callback = [=]() {
if (delegate) {
delegate->OnWake();
}
};
}
multiple_io_base_->PollSetAdd(pollfd);
}
void IOLoop::RemoveDelegateAsync(socket_t sock) {
PollFd pollfd = {};
pollfd.fd = sock;
pollfd.event = READBLE_EVENT;
multiple_io_base_->PollSetRemove(pollfd);
}
} // namespace lidar
} // namespace livox
sdk_core/base/io_loop.h
+92
View File
@@ -0,0 +1,92 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_IO_LOOP_H_
#define LIVOX_IO_LOOP_H_
#include <functional>
#include <mutex>
#include <unordered_map>
#include <utility>
#include <vector>
#include <algorithm>
#include "command_callback.h"
#include "noncopyable.h"
#include "thread_base.h"
#include "multiple_io/multiple_io_base.h"
#include "multiple_io/multiple_io_factory.h"
namespace livox {
namespace lidar {
#define OPEN_MAX_POLL 48
#define POLL_TIMEOUT 50 //ms
typedef int socket_t;
class IOLoop : public noncopyable {
public:
typedef std::function<void(void)> IOLoopTask;
class IOLoopDelegate {
public:
virtual void OnData(socket_t, void *) {}
virtual void OnTimer(std::chrono::steady_clock::time_point) {}
virtual void OnWake() {}
};
public:
explicit IOLoop(bool enable_timer = true, bool enable_wake = true)
: enable_timer_(enable_timer), enable_wake_(enable_wake){};
bool Init();
void Uninit();
void AddDelegate(socket_t sock, IOLoopDelegate *delegate, void *data = NULL);
void RemoveDelegate(socket_t sock, IOLoopDelegate *delegate);
void Loop();
bool Wakeup();
void PostTask(const IOLoopTask &task);
private:
void AddDelegateAsync(socket_t sock, IOLoopDelegate *delegate, void *data);
void RemoveDelegateAsync(socket_t sock);
private:
std::mutex mutex_;
bool enable_timer_;
bool enable_wake_;
std::vector<IOLoopTask> pending_tasks_;
std::unique_ptr<MultipleIOBase> multiple_io_base_;
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_IO_LOOP_H_
sdk_core/base/io_thread.cpp
+56
View File
@@ -0,0 +1,56 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "io_thread.h"
namespace livox {
namespace lidar {
IOThread::~IOThread() {
Join();
Uninit();
}
void IOThread::ThreadFunc() {
if (!loop_) {
return;
}
while (!IsQuit()) {
loop_->Loop();
}
}
bool IOThread::Init(bool enable_timer, bool enable_wake) {
loop_ = std::make_shared<IOLoop>(enable_timer, enable_wake);
return loop_->Init();
}
void IOThread::Uninit() {
if (loop_) {
loop_->Uninit();
}
}
} // namespace lidar
} // namespace livox
sdk_core/base/io_thread.h
+49
View File
@@ -0,0 +1,49 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_IO_THREAD_H_
#define LIVOX_IO_THREAD_H_
#include <memory>
#include "io_loop.h"
#include "thread_base.h"
namespace livox {
namespace lidar {
class IOThread : public ThreadBase {
public:
IOThread() : loop_(nullptr) {}
virtual ~IOThread();
bool Init(bool enable_timer = true, bool enable_wake = true);
std::weak_ptr<IOLoop> GetLoop() { return loop_; }
void ThreadFunc();
private:
void Uninit();
std::shared_ptr<IOLoop> loop_;
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_IO_THREAD_H_
sdk_core/base/logging.cpp
+59
View File
@@ -0,0 +1,59 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "logging.h"
std::shared_ptr<spdlog::logger> logger = NULL;
bool is_save_log_file = false;
bool is_console_log_enable = true;
void InitLogger() {
if (spdlog::get("console") != nullptr) {
logger = spdlog::get("console");
return;
}
std::vector<spdlog::sink_ptr> sinkList;
if (is_console_log_enable) {
auto consoleSink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
consoleSink->set_level(spdlog::level::debug);
sinkList.push_back(consoleSink);
}
if (is_save_log_file) {
auto rotateSink = std::make_shared<spdlog::sinks::rotating_file_sink_mt>("livox_log.txt", 1024 * 1024 * 5, 2);
rotateSink->set_level(spdlog::level::debug);
sinkList.push_back(rotateSink);
}
logger = std::make_shared<spdlog::logger>("console", begin(sinkList), end(sinkList));
spdlog::register_logger(logger);
logger->set_level(spdlog::level::debug);
logger->flush_on(spdlog::level::debug);
}
void UninitLogger() {
spdlog::drop_all();
}
sdk_core/base/logging.h
+66
View File
@@ -0,0 +1,66 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_LOGGING_H_
#define LIVOX_LOGGING_H_
#include "spdlog/spdlog.h"
#include "spdlog/sinks/stdout_color_sinks.h"
#include "spdlog/sinks/rotating_file_sink.h"
#ifdef _WIN32
#define __FILENAME__ (strrchr(__FILE__, '\\') ? (strrchr(__FILE__, '\\') + 1):__FILE__)
#else
#define __FILENAME__ (strrchr(__FILE__, '/') ? (strrchr(__FILE__, '/') + 1):__FILE__)
#endif
#ifndef suffix
#define suffix(msg) std::string(msg).append(" [")\
.append(__FILENAME__).append("] [").append(__func__)\
.append("] [").append(std::to_string(__LINE__))\
.append("]").c_str()
#endif
#ifndef SPDLOG_TRACE_ON
#define SPDLOG_TRACE_ON
#endif
#ifndef SPDLOG_DEBUG_ON
#define SPDLOG_DEBUG_ON
#endif
extern std::shared_ptr<spdlog::logger> logger;
extern bool is_save_log_file;
extern bool is_console_log_enable;
void InitLogger();
void UninitLogger();
#define LOG_TRACE(msg, ...) logger->trace(suffix(msg), ##__VA_ARGS__)
#define LOG_DEBUG(msg, ...) logger->debug(suffix(msg), ##__VA_ARGS__)
#define LOG_INFO(msg, ...) logger->info(suffix(msg), ##__VA_ARGS__)
#define LOG_WARN(msg, ...) logger->warn(suffix(msg), ##__VA_ARGS__)
#define LOG_ERROR(msg, ...) logger->error(suffix(msg), ##__VA_ARGS__)
#define LOG_FATAL(msg, ...) logger->critical(suffix(msg), ##__VA_ARGS__)
#endif // LIVOX_LOGGING_H_
sdk_core/base/multiple_io/multiple_io_base.cpp
+86
View File
@@ -0,0 +1,86 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "multiple_io_base.h"
namespace livox {
namespace lidar {
void MultipleIOBase::CheckTimer() {
TimePoint t = std::chrono::steady_clock::now();
if (t - last_timeout_ > std::chrono::milliseconds(50)) {
last_timeout_ = t;
for (auto & descriptor : descriptors_) {
PollFd pollfd = descriptor.second;
if (pollfd.timer_callback) {
pollfd.timer_callback(t);
}
}
}
}
void MultipleIOBase::WakeUpInit() {
//Initialize wake up pipe
wake_up_pipe_.reset(new WakeUpPipe());
wake_up_pipe_->PipeCreate();
//register wake_fd to multiple io
PollFd wake_fd = {};
wake_fd.fd = wake_up_pipe_->GetPipeOut();
wake_fd.event = READBLE_EVENT;
wake_fd.event_callback = [this](FdEvent event) {
if (event & READBLE_EVENT) {
if (wake_up_pipe_) {
wake_up_pipe_->Drain();
}
for (auto & descriptor : descriptors_) {
PollFd pollfd = descriptor.second;
if (pollfd.wake_callback) {
pollfd.wake_callback();
}
}
}
};
PollSetAdd(wake_fd);
}
void MultipleIOBase::WakeUpUninit() {
PollFd wake_fd = {};
wake_fd.fd = wake_up_pipe_->GetPipeOut();
wake_fd.event = READBLE_EVENT | WRITABLE_EVENT;
PollSetRemove(wake_fd);
if (wake_up_pipe_) {
wake_up_pipe_->PipeDestroy();
wake_up_pipe_ = nullptr;
}
}
void MultipleIOBase::PollWakeUp() {
if (wake_up_pipe_) {
wake_up_pipe_->WakeUp();
}
return;
}
} // namespace lidar
} // namespace livox
sdk_core/base/multiple_io/multiple_io_base.h
+75
View File
@@ -0,0 +1,75 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef MULTIPLE_IO_BASE_H_
#define MULTIPLE_IO_BASE_H_
#include <map>
#include <functional>
#include <chrono>
#include <memory>
#include "base/wake_up/wake_up_pipe.h"
namespace livox {
namespace lidar {
#define NONE_EVENT 0 /* No events registered. */
#define READBLE_EVENT 1 /* when descriptor is readable. */
#define WRITABLE_EVENT 2 /* when descriptor is writeable. */
typedef std::chrono::steady_clock::time_point TimePoint;
typedef int FdEvent;
typedef struct {
int fd; /* File descriptor. */
FdEvent event; /* Read | Write Event to listen. */
std::function<void(FdEvent)> event_callback; /* Read or Write Event Callback. */
std::function<void(TimePoint)> timer_callback; /* Timer Event Callback. */
std::function<void()> wake_callback; /* WakeUp Event Callback. */
} PollFd;
class MultipleIOBase {
public:
MultipleIOBase() = default;
virtual ~MultipleIOBase() = default;
virtual bool PollCreate(int size) = 0;
virtual void PollDestroy() = 0;
virtual bool PollSetAdd(PollFd poll_fd) = 0;
virtual bool PollSetRemove(PollFd poll_fd) = 0;
virtual void Poll(int timeout) = 0;
virtual void PollWakeUp();
protected:
virtual void CheckTimer();
std::map<int, PollFd> descriptors_;
TimePoint last_timeout_ = TimePoint();
virtual void WakeUpInit();
virtual void WakeUpUninit();
std::unique_ptr<WakeUpPipe> wake_up_pipe_;
};
} // namespace lidar
} // namespace livox
#endif // MULTIPLE_IO_BASE_H_
sdk_core/base/multiple_io/multiple_io_epoll.cpp
+109
View File
@@ -0,0 +1,109 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "multiple_io_epoll.h"
#ifdef HAVE_EPOLL
namespace livox {
namespace lidar {
int GetEvent (FdEvent event) {
FdEvent rv = 0;
if (event & READBLE_EVENT)
rv |= EPOLLIN;
if (event & WRITABLE_EVENT)
rv |= EPOLLOUT;
return rv;
}
bool MultipleIOEpoll::PollCreate(int size) {
max_poll_size_ = size + 1;
epoll_fd_ = epoll_create(max_poll_size_);
if (epoll_fd_ < 0) {
return false;
}
pollset_.reset(new struct epoll_event[size]);
WakeUpInit();
return true;
}
void MultipleIOEpoll::PollDestroy() {
WakeUpUninit();
if (epoll_fd_ > 0) {
close(epoll_fd_);
epoll_fd_ = -1;
}
}
bool MultipleIOEpoll::PollSetAdd(PollFd poll_fd) {
if (max_poll_size_ <= (int)descriptors_.size()) {
return false;
}
struct epoll_event ee = {0};
ee.events = GetEvent(poll_fd.event);
ee.data.fd = poll_fd.fd;
if (epoll_ctl(epoll_fd_, EPOLL_CTL_ADD, poll_fd.fd, &ee) == -1) {
return false;
}
int fd = poll_fd.fd;
descriptors_[fd] = poll_fd;
return true;
}
bool MultipleIOEpoll::PollSetRemove(PollFd poll_fd) {
int fd = poll_fd.fd;
struct epoll_event ee = {0};
epoll_ctl(epoll_fd_, EPOLL_CTL_DEL, fd, &ee);
if (descriptors_.find(fd) != descriptors_.end()) {
descriptors_.erase(fd);
}
return true;
}
void MultipleIOEpoll::Poll(int time_out) {
int ret = epoll_wait(epoll_fd_, pollset_.get(), (int)descriptors_.size(),
time_out);
if (ret > 0) {
for (int i =0; i< ret; i++) {
FdEvent fd_event = NONE_EVENT;
if (pollset_[i].events & EPOLLIN) {
fd_event |= READBLE_EVENT;
}
if (pollset_[i].events & EPOLLOUT) {
fd_event |= WRITABLE_EVENT;
}
int fd = pollset_[i].data.fd;
if (descriptors_.find(fd) != descriptors_.end()) {
PollFd pollfd = descriptors_[fd];
pollfd.event_callback(fd_event);
}
}
}
CheckTimer();
}
} // namespace lidar
} // namespace livox
#endif // HAVE_EPOLL
sdk_core/base/multiple_io/multiple_io_epoll.h
+54
View File
@@ -0,0 +1,54 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef MULTIPLE_IO_EPOLL_H_
#define MULTIPLE_IO_EPOLL_H_
#include "multiple_io_base.h"
#include "livox_lidar_cfg.h"
#include <memory>
#ifdef HAVE_EPOLL
namespace livox {
namespace lidar {
class MultipleIOEpoll : public MultipleIOBase {
public:
bool PollCreate(int size);
bool PollSetAdd(PollFd poll_fd);
bool PollSetRemove(PollFd poll_fd);
void Poll(int timeout);
void PollDestroy();
private:
int epoll_fd_ = -1;
std::unique_ptr<struct epoll_event[]> pollset_;
int max_poll_size_ = 0;
};
} // namespace lidar
} // namespace livox
#endif // HAVE_EPOLL
#endif // MULTIPLE_IO_EPOLL_H_
sdk_core/base/multiple_io/multiple_io_factory.h
+58
View File
@@ -0,0 +1,58 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef MULTIPLE_IO_FACTORY_H_
#define MULTIPLE_IO_FACTORY_H_
#include "multiple_io_base.h"
#include "multiple_io_epoll.h"
#include "multiple_io_kqueue.h"
#include "multiple_io_select.h"
#include "multiple_io_poll.h"
#include <memory>
namespace livox {
namespace lidar {
class MultipleIOFactory {
public:
static std::unique_ptr<MultipleIOBase> CreateMultipleIO() {
#if defined(HAVE_EPOLL)
return std::unique_ptr<MultipleIOBase>(new MultipleIOEpoll());
#elif defined(HAVE_KQUEUE)
return std::unique_ptr<MultipleIOBase>(new MultipleIOKqueue());
#elif defined(HAVE_SELECT)
return std::unique_ptr<MultipleIOBase>(new MultipleIOSelect());
#elif defined(HAVE_POLL)
return std::unique_ptr<MultipleIOBase>(new MultipleIOPoll());
#else
return nullptr;
#endif
}
};
} // namespace lidar
} // namespace livox
#endif // MULTIPLE_IO_FACTORY_H_
sdk_core/base/multiple_io/multiple_io_kqueue.cpp
+142
View File
@@ -0,0 +1,142 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "multiple_io_kqueue.h"
#ifdef HAVE_KQUEUE
namespace livox {
namespace lidar {
bool MultipleIOKqueue::PollCreate(int size) {
kqueue_fd_ = kqueue();
if (kqueue_fd_ == -1) {
return false;
}
int flags = 0;
if ((flags = fcntl(kqueue_fd_, F_GETFD)) == -1) {
close(kqueue_fd_);
return false;
}
flags |= FD_CLOEXEC;
if (fcntl(kqueue_fd_, F_SETFD, flags) == -1) {
close(kqueue_fd_);
return false;
}
max_poll_size_ = size + 1;
set_size_ = 2 * max_poll_size_;
kevent_set_.reset(new struct kevent[set_size_]);
WakeUpInit();
return true;
}
void MultipleIOKqueue::PollDestroy() {
WakeUpUninit();
if (kqueue_fd_ > 0) {
close(kqueue_fd_);
kqueue_fd_ = -1;
}
}
bool MultipleIOKqueue::PollSetAdd(PollFd poll_fd) {
if (max_poll_size_ <= (int)descriptors_.size()) {
return false;
}
int fd = poll_fd.fd;
descriptors_[fd] = poll_fd;
if (poll_fd.event & READBLE_EVENT) {
EV_SET(&kevent_, fd, EVFILT_READ, EV_ADD, 0, 0, &descriptors_[fd].fd);
if (kevent(kqueue_fd_, &kevent_, 1, nullptr, 0, nullptr) == -1) {
descriptors_.erase(fd);
return false;
}
}
if (poll_fd.event & WRITABLE_EVENT) {
EV_SET(&kevent_, fd, EVFILT_WRITE, EV_ADD, 0, 0, &descriptors_[fd].fd);
if (kevent(kqueue_fd_, &kevent_, 1, nullptr, 0, nullptr) == -1) {
descriptors_.erase(fd);
return false;
}
}
return true;
}
bool MultipleIOKqueue::PollSetRemove(PollFd poll_fd) {
int fd = poll_fd.fd;
bool result = true;
if (descriptors_.find(fd) != descriptors_.end()) {
do {
if (descriptors_[fd].event & READBLE_EVENT) {
EV_SET(&kevent_, fd, EVFILT_READ, EV_DELETE, 0, 0, NULL);
if (kevent(kqueue_fd_, &kevent_, 1, nullptr, 0, nullptr) == -1) {
result = false;
break;
}
}
if (descriptors_[fd].event & WRITABLE_EVENT) {
EV_SET(&kevent_, fd, EVFILT_WRITE, EV_DELETE, 0, 0, NULL);
if (kevent(kqueue_fd_, &kevent_, 1, nullptr, 0, nullptr) == -1) {
result = false;
break;
}
}
} while(0);
descriptors_.erase(fd);
}
return result;
}
void MultipleIOKqueue::Poll(int time_out) {
struct timespec tv, *tvptr;
if (time_out < 0) {
tvptr = NULL;
} else {
tv.tv_sec = (long) time_out / 1000;
tv.tv_nsec = (long) (time_out % 1000) * 1000000;
tvptr = &tv;
}
int rv = kevent(kqueue_fd_, NULL, 0, kevent_set_.get(), set_size_, tvptr);
if (rv > 0) {
for (int i = 0; i < rv; i++) {
int fd = *(int *)(kevent_set_[i].udata);
if (descriptors_.find(fd) != descriptors_.end()) {
PollFd pollfd = descriptors_[fd];
if (kevent_set_[i].filter == EVFILT_READ) {
pollfd.event_callback(READBLE_EVENT);
}
if (kevent_set_[i].filter == EVFILT_WRITE) {
pollfd.event_callback(WRITABLE_EVENT);
}
}
}
}
CheckTimer();
}
} // namespace lidar
} // namespace livox
#endif // HAVE_KQUEUE
sdk_core/base/multiple_io/multiple_io_kqueue.h
+54
View File
@@ -0,0 +1,54 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef MULTIPLE_IO_KQUEUE_H_
#define MULTIPLE_IO_KQUEUE_H_
#include "multiple_io_base.h"
#include "livox_lidar_cfg.h"
#ifdef HAVE_KQUEUE
namespace livox {
namespace lidar {
class MultipleIOKqueue : public MultipleIOBase {
public:
bool PollCreate(int size);
bool PollSetAdd(PollFd poll_fd);
bool PollSetRemove(PollFd poll_fd);
void Poll(int timeout);
void PollDestroy();
private:
int kqueue_fd_ = -1;
struct kevent kevent_ = {};
std::unique_ptr<struct kevent[]> kevent_set_;
int max_poll_size_ = 0;
int set_size_ = 0;
};
} // namespace lidar
} // namespace livox
#endif // HAVE_KQUEUE
#endif // MULTIPLE_IO_KQUEUE_H_
sdk_core/base/multiple_io/multiple_io_poll.cpp
+116
View File
@@ -0,0 +1,116 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "multiple_io_poll.h"
#ifdef HAVE_POLL
namespace livox {
namespace lidar {
int GetEvent (FdEvent event) {
int rv = 0;
if (event & READBLE_EVENT)
rv |= POLLIN;
if (event & WRITABLE_EVENT)
rv |= POLLOUT;
return rv;
}
bool MultipleIOPoll:: PollCreate(int size) {
max_poll_size_ = size + 1;
pollset_.reset(new struct pollfd[max_poll_size_]);
WakeUpInit();
return true;
}
bool MultipleIOPoll:: PollSetAdd(PollFd poll_fd) {
if (max_poll_size_ <= (int)descriptors_.size()) {
return false;
}
int fd = poll_fd.fd;
descriptors_[fd] = poll_fd;
struct pollfd fds;
fds.fd = fd;
fds.events = GetEvent(poll_fd.event);
pollset_[pollset_num_] = fds;
pollset_num_++;
return true;
}
void MultipleIOPoll::PollDestroy() {
WakeUpUninit();
return;
}
bool MultipleIOPoll:: PollSetRemove(PollFd poll_fd) {
int fd = poll_fd.fd;
if (descriptors_.find(fd) != descriptors_.end()) {
descriptors_.erase(fd);
}
for (int i = 0; i< pollset_num_; i++) {
if (pollset_[i].fd == fd) {
int dst = i;
for (i++; i < pollset_num_ - 1; i++) {
if (pollset_[i].fd == fd) {
pollset_num_--;
} else {
pollset_[dst] = pollset_[i];
dst++;
}
}
}
}
return true;
}
void MultipleIOPoll:: Poll(int time_out) {
int rv = poll(pollset_.get(), pollset_num_, time_out);
if (rv > 0) {
for (int i = 0; i < pollset_num_; i++) {
FdEvent fd_event = NONE_EVENT;
if (pollset_[i].revents & POLLIN) {
fd_event |= READBLE_EVENT;
}
if (pollset_[i].revents & POLLOUT) {
fd_event |= WRITABLE_EVENT;
}
int fd = pollset_[i].fd;
if (descriptors_.find(fd) != descriptors_.end()) {
PollFd pollfd = descriptors_[fd];
pollfd.event_callback(fd_event);
}
pollset_[i].revents = NONE_EVENT;
}
}
CheckTimer();
}
} // namespace lidar
} // namespace livox
#endif // HAVE_POLL
sdk_core/base/multiple_io/multiple_io_poll.h
+55
View File
@@ -0,0 +1,55 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef MULTIPLE_IO_POLL_H_
#define MULTIPLE_IO_POLL_H_
#include "multiple_io_base.h"
#include "livox_lidar_cfg.h"
#include <memory>
#ifdef HAVE_POLL
namespace livox {
namespace lidar {
class MultipleIOPoll : public MultipleIOBase {
public:
bool PollCreate(int size);
bool PollSetAdd(PollFd poll_fd);
bool PollSetRemove(PollFd poll_fd);
void Poll(int timeout);
void PollDestroy();
private:
std::unique_ptr<struct pollfd[]> pollset_;
int pollset_num_ = 0;
int max_poll_size_ = 0;
};
} // namespace lidar
} // namespace livox
#endif // HAVE_POLL
#endif // MULTIPLE_IO_POLL_H_
sdk_core/base/multiple_io/multiple_io_select.cpp
+123
View File
@@ -0,0 +1,123 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "multiple_io_select.h"
#include <thread>
#ifdef HAVE_SELECT
namespace livox {
namespace lidar {
bool MultipleIOSelect:: PollCreate(int size) {
FD_ZERO(&rfds_);
FD_ZERO(&wfds_);
//wake up fd + 1
max_poll_size_ = size + 1;
WakeUpInit();
return true;
}
void MultipleIOSelect::PollDestroy() {
WakeUpUninit();
FD_ZERO(&rfds_);
FD_ZERO(&wfds_);
}
bool MultipleIOSelect:: PollSetAdd(PollFd poll_fd) {
if (max_poll_size_ <= (int)descriptors_.size()) {
return false;
}
int fd = poll_fd.fd;
descriptors_[fd] = poll_fd;
if (max_fd_ < fd) {
max_fd_ = fd;
}
if (poll_fd.event & READBLE_EVENT) {
FD_SET(fd, &rfds_);
}
if (poll_fd.event & WRITABLE_EVENT) {
FD_SET(fd, &wfds_);
}
return true;
}
bool MultipleIOSelect:: PollSetRemove(PollFd poll_fd) {
int fd = poll_fd.fd;
if (descriptors_.find(fd) != descriptors_.end()) {
descriptors_.erase(fd);
}
FD_CLR(fd, &rfds_);
FD_CLR(fd, &wfds_);
if (max_fd_ <= fd) {
max_fd_--;
}
return true;
}
void MultipleIOSelect:: Poll(int time_out) {
fd_set readset, writeset;
struct timeval tv, *tvptr;
if (descriptors_.size() == 0) {
if (time_out > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(time_out));
return;
}
return ;
}
if (time_out < 0) {
tvptr = nullptr;
} else {
tv.tv_sec = (long)time_out / 1000;
tv.tv_usec = (long)time_out % 1000;
tvptr = &tv;
}
memcpy(&readset, &rfds_, sizeof(fd_set));
memcpy(&writeset, &wfds_, sizeof(fd_set));
int rv = select(max_fd_ + 1, &readset, &writeset, nullptr, tvptr);
if (rv > 0) {
for (auto& descriptor : descriptors_) {
int fd = descriptor.first;
FdEvent fd_event = NONE_EVENT;
if (FD_ISSET(fd, &readset)) {
fd_event |= READBLE_EVENT;
}
if (FD_ISSET(fd, &writeset)) {
fd_event |= WRITABLE_EVENT;
}
if (fd_event != NONE_EVENT) {
PollFd pollfd = descriptor.second;
pollfd.event_callback(fd_event);
}
}
}
CheckTimer();
}
} // namespace lidar
} // namespace livox
#endif // HAVE_SELECT
sdk_core/base/multiple_io/multiple_io_select.h
+54
View File
@@ -0,0 +1,54 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef MULTIPLE_IO_SELECT_H_
#define MULTIPLE_IO_SELECT_H_
#include "multiple_io_base.h"
#include "livox_lidar_cfg.h"
#ifdef HAVE_SELECT
namespace livox {
namespace lidar {
class MultipleIOSelect : public MultipleIOBase {
public:
bool PollCreate(int size);
bool PollSetAdd(PollFd poll_fd);
bool PollSetRemove(PollFd poll_fd);
void Poll(int timeout);
void PollDestroy();
private:
int max_fd_ = -1;
fd_set rfds_;
fd_set wfds_;
int max_poll_size_ = 0;
};
} // namespace lidar
} // namespace livox
#endif // HAVE_SELECT
#endif // MULTIPLE_IO_SELECT_H_
sdk_core/base/network/network_util.h
+62
View File
@@ -0,0 +1,62 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_NETWORK_UTIL_H_
#define LIVOX_NETWORK_UTIL_H_
#include <stdint.h>
#ifdef WIN32
#include <winsock2.h>
#include <Ws2tcpip.h>
#else
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#endif // WIN32
#include <stdio.h>
#include <stdio.h>
#include <fcntl.h>
#include <string>
namespace livox {
namespace lidar {
namespace util {
typedef int socket_t;
socket_t CreateSocket(uint16_t port, bool nonblock = true, bool reuse_port = true, bool is_broadcast = false, const std::string netif = "", const std::string multicast_ip = "");
//socket_t CreateSocket(uint16_t port, bool nonblock = true, bool reuse_port = true, bool is_broadcast = false);
void CloseSock(socket_t sock);
bool FindLocalIp(const struct sockaddr_in &client_addr, uint32_t &local_ip);
size_t RecvFrom(socket_t &sock, void *buff, size_t buf_size, int flag, struct sockaddr *addr, int* addrlen);
} // namespace util
} // namespace lidar
} // namespace livox
#endif // LIVOX_NETWORK_UTIL_H_
sdk_core/base/network/unix/network_util.cpp
+218
View File
@@ -0,0 +1,218 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef WIN32
#include "base/network/network_util.h"
#include <ifaddrs.h>
#include <string>
#include <string.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <netdb.h>
namespace livox {
namespace lidar {
namespace util {
socket_t CreateSocket(uint16_t port, bool nonblock, bool reuse_port, bool is_broadcast, const std::string netif, const std::string multicast_ip) {
int status = -1;
int on = -1;
int sock = -1;
int recv_buff_size = 1024 * 1024 * 200;
struct sockaddr_in servaddr;
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock < 0) {
printf("create failed\n");
return -1;
}
if (nonblock) {
status = ioctl(sock, FIONBIO, (char*)&on);
if (status != 0) {
printf("noblock failed\n");
close(sock);
return -1;
}
}
if (reuse_port) {
status = setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
(char *) &on, sizeof (on));
if (status != 0) {
printf("reuse port failed\n");
close(sock);
return -1;
}
}
status = setsockopt(sock, SOL_SOCKET, SO_RCVBUF,
(char *)&recv_buff_size, sizeof(recv_buff_size));
if (status != 0) {
close(sock);
return -1;
}
memset(&servaddr, 0, sizeof(servaddr));
// Filling server information
servaddr.sin_family = AF_INET; // IPv4
if (netif.empty()) {
servaddr.sin_addr.s_addr = INADDR_ANY;
} else {
if(!multicast_ip.empty()){
servaddr.sin_addr.s_addr = inet_addr(multicast_ip.c_str());
} else {
servaddr.sin_addr.s_addr = inet_addr(netif.c_str());
}
}
servaddr.sin_port = htons(port);
status = bind(sock, (const struct sockaddr *)&servaddr, sizeof(servaddr));
if (status != 0) {
printf("bind failed\n");
close(sock);
return -1;
}
if (is_broadcast) {
status = setsockopt(sock, SOL_SOCKET, SO_BROADCAST,
(char *)&on, sizeof(on));
if (status != 0) {
close(sock);
printf("broad cast failed\n");
return -1;
}
}
if (!multicast_ip.empty()) {
struct ip_mreq mreq;
bzero(&mreq, sizeof(struct ip_mreq));
mreq.imr_interface.s_addr = inet_addr(netif.c_str());
mreq.imr_multiaddr.s_addr = inet_addr(multicast_ip.c_str());
if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(struct ip_mreq)) == -1) {
printf("setsockopt failed\n");
}
}
return sock;
}
// socket_t CreateSocket(uint16_t port, bool nonblock, bool reuse_port, bool is_broadcast) {
// int status = -1;
// int on = -1;
// int sock = -1;
// struct sockaddr_in servaddr;
// sock = socket(AF_INET, SOCK_DGRAM, 0);
// if (sock < 0) {
// return -1;
// }
// if (nonblock) {
// status = ioctl(sock, FIONBIO, (char*)&on);
// if (status != 0) {
// close(sock);
// return -1;
// }
// }
// if (reuse_port) {
// status = setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
// (char *) &on, sizeof (on));
// if (status != 0) {
// close(sock);
// return -1;
// }
// }
// if (is_broadcast) {
// status = setsockopt(sock, SOL_SOCKET, SO_BROADCAST,
// (char *)&on, sizeof(on));
// if (status != 0) {
// close(sock);
// return -1;
// }
// }
// memset(&servaddr, 0, sizeof(servaddr));
// // Filling server information
// servaddr.sin_family = AF_INET; // IPv4
// servaddr.sin_addr.s_addr = INADDR_ANY;
// servaddr.sin_port = htons(port);
// status = bind(sock, (const struct sockaddr *)&servaddr, sizeof(servaddr));
// if (status != 0) {
// close(sock);
// return -1;
// }
// return sock;
// }
void CloseSock(int sock) {
if (sock > 0) {
close(sock);
}
}
bool FindLocalIp(const struct sockaddr_in &client_addr, uint32_t &local_ip) {
struct ifaddrs *if_addrs = NULL, *addrs = NULL;
if (getifaddrs(&if_addrs) == -1) {
return false;
}
addrs = if_addrs;
bool found = false;
while (if_addrs != NULL) {
if ((if_addrs->ifa_addr != NULL) && (if_addrs->ifa_addr->sa_family == AF_INET)) // check it is IP4
{
// is a connected IP4 Address
struct sockaddr_in *ifu_localaddr = (struct sockaddr_in *)if_addrs->ifa_addr;
struct sockaddr_in *ifu_netmask = (struct sockaddr_in *)if_addrs->ifa_netmask;
if (ifu_localaddr->sin_addr.s_addr != htonl(INADDR_ANY)) {
if ((ifu_localaddr->sin_addr.s_addr & ifu_netmask->sin_addr.s_addr) ==
(client_addr.sin_addr.s_addr & ifu_netmask->sin_addr.s_addr)) {
local_ip = ifu_localaddr->sin_addr.s_addr;
found = true;
break;
}
}
}
if_addrs = if_addrs->ifa_next;
}
if (addrs) {
freeifaddrs(addrs);
}
return found;
}
size_t RecvFrom(socket_t &sock, void *buff, size_t buf_size, int flag, struct sockaddr *addr, int *addrlen) {
return recvfrom(sock, buff, buf_size, 0, addr, (socklen_t *)addrlen);
}
} // namespace util
} // namespace lidar
} // namespace livox
#endif // WIN32
sdk_core/base/network/win/network_util.cpp
+177
View File
@@ -0,0 +1,177 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifdef WIN32
#include "base/network/network_util.h"
#include <memory>
#include <iphlpapi.h>
#include <string>
#pragma comment(lib,"iphlpapi.lib")
#pragma comment(lib,"ws2_32.lib")
namespace livox {
namespace lidar {
namespace util {
void CloseSock(socket_t sock) {
closesocket(sock);
}
socket_t CreateSocket(uint16_t port, bool nonblock, bool reuse_port, bool is_broadcast, std::string netif, const std::string multicast_ip) {
int status = -1;
int on = -1;
int sock = -1;
int recv_buff_size = 1024 * 1024 * 200;
struct sockaddr_in servaddr;
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock == INVALID_SOCKET) {
return -1;
}
if (nonblock) {
status = ioctlsocket(sock, FIONBIO, (u_long *)&on);
if (status != NO_ERROR) {
closesocket(sock);
return -1;
}
}
if (reuse_port) {
status = setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
(char *) &on, sizeof (on));
if (status != 0) {
closesocket(sock);
return -1;
}
}
if (is_broadcast) {
status = setsockopt(sock, SOL_SOCKET, SO_BROADCAST,
(char *)&on, sizeof(on));
if (status != 0) {
closesocket(sock);
return -1;
}
}
status = setsockopt(sock, SOL_SOCKET, SO_RCVBUF,
(char *)&recv_buff_size, sizeof(recv_buff_size));
if (status != 0) {
closesocket(sock);
return -1;
}
memset(&servaddr, 0, sizeof(servaddr));
// Filling server information
servaddr.sin_family = AF_INET; // IPv4
if (netif.empty()) {
servaddr.sin_addr.s_addr = INADDR_ANY;
} else {
servaddr.sin_addr.s_addr = inet_addr(netif.c_str());
}
servaddr.sin_port = htons(port);
status = bind(sock, (const struct sockaddr *)&servaddr, sizeof(servaddr));
if (status != 0) {
closesocket(sock);
return -1;
}
if (is_broadcast) {
status = setsockopt(sock, SOL_SOCKET, SO_BROADCAST,
(char *)&on, sizeof(on));
if (status != 0) {
closesocket(sock);
printf("broad cast failed\n");
return -1;
}
}
if (!multicast_ip.empty()) {
struct ip_mreq mreq;
memset(&mreq, 0, sizeof(struct ip_mreq));
mreq.imr_interface.s_addr = inet_addr(netif.c_str());
mreq.imr_multiaddr.s_addr = inet_addr(multicast_ip.c_str());
if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, reinterpret_cast<char*>(&mreq), sizeof(struct ip_mreq)) == -1) {
printf("setsockopt failed\n");
}
}
return sock;
}
bool GetAdapterState(const IP_ADAPTER_INFO *pAdapter) {
if(pAdapter == NULL) {
return false;
}
MIB_IFROW info;
memset(&info ,0 ,sizeof(MIB_IFROW));
info.dwIndex = pAdapter->Index;
if (GetIfEntry(&info) != NOERROR) {
return false;
}
if (info.dwOperStatus == IF_OPER_STATUS_NON_OPERATIONAL
|| info.dwOperStatus == IF_OPER_STATUS_UNREACHABLE
|| info.dwOperStatus == IF_OPER_STATUS_DISCONNECTED
|| info.dwOperStatus == IF_OPER_STATUS_CONNECTING)
return false;
return true;
}
bool FindLocalIp(const struct sockaddr_in &client_addr, uint32_t &local_ip) {
bool found = false;
ULONG ulOutbufLen = sizeof(IP_ADAPTER_INFO);
std::unique_ptr<uint8_t[]> pAdapterInfo(new uint8_t[ulOutbufLen]);
DWORD dlRetVal = GetAdaptersInfo(reinterpret_cast<IP_ADAPTER_INFO *>(pAdapterInfo.get()), &ulOutbufLen);
if (dlRetVal == ERROR_BUFFER_OVERFLOW) {
pAdapterInfo.reset(new uint8_t[ulOutbufLen]);
dlRetVal = GetAdaptersInfo(reinterpret_cast<IP_ADAPTER_INFO *>(pAdapterInfo.get()), &ulOutbufLen);
}
IP_ADAPTER_INFO *pAdapter = reinterpret_cast<IP_ADAPTER_INFO *>(pAdapterInfo.get());
if (NO_ERROR == dlRetVal && pAdapter != NULL) {
while (pAdapter != NULL) {
if (GetAdapterState(pAdapter)) {
std::string str_ip = pAdapter->IpAddressList.IpAddress.String;
std::string str_mask = pAdapter->IpAddressList.IpMask.String;
ULONG host_ip = inet_addr(const_cast<char *>(str_ip.c_str()));
ULONG host_mask = inet_addr(const_cast<char *>(str_mask.c_str()));
if ((host_ip & host_mask) ==
(client_addr.sin_addr.S_un.S_addr & host_mask)) {
local_ip = host_ip;
found = true;
break;
}
}
pAdapter = pAdapter->Next;
}
}
return found;
}
size_t RecvFrom(socket_t &sock, void *buff, size_t buf_size, int flag, struct sockaddr *addr, int* addrlen) {
return recvfrom(sock, (char *)buff, buf_size, 0, addr, addrlen);
}
} // namespace util
} // namespace lidar
} // namespace livox
#endif // WIN32
sdk_core/base/noncopyable.h
+43
View File
@@ -0,0 +1,43 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_NONCOPYABLE_H_
#define LIVOX_NONCOPYABLE_H_
namespace livox {
namespace lidar {
class noncopyable {
protected:
noncopyable() {}
~noncopyable() {}
private:
noncopyable(const noncopyable &);
noncopyable &operator=(const noncopyable &);
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_NONCOPYABLE_H_
sdk_core/base/thread_base.cpp
+61
View File
@@ -0,0 +1,61 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "thread_base.h"
#include <thread>
#include <iostream>
#include <memory>
namespace livox {
namespace lidar {
ThreadBase::ThreadBase() : quit_(false) {}
bool ThreadBase::Start() {
quit_ = false;
thread_ = std::make_shared<std::thread>(&ThreadBase::ThreadFunc, this);
return true;
}
ThreadBase::~ThreadBase() {
if (thread_) {
Join();
}
}
void ThreadBase::Join() {
quit_ = true;
if (thread_ && thread_->joinable()) {
thread_->join();
thread_ = nullptr;
} else {
std::cout << "failed to join thread, joinable: "
<< thread_->joinable() << std::endl;
thread_ = nullptr;
}
}
} // namespace lidar
} // namespace livox
sdk_core/base/thread_base.h
+54
View File
@@ -0,0 +1,54 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_THREAD_BASE_H_
#define LIVOX_THREAD_BASE_H_
#include <atomic>
#include <thread>
#include <memory>
#include "noncopyable.h"
namespace livox {
namespace lidar {
class ThreadBase : public noncopyable {
public:
ThreadBase();
virtual ~ThreadBase();
virtual void ThreadFunc() = 0;
bool Start();
bool IsQuit() { return quit_; }
protected:
void Join();
private:
std::atomic_bool quit_;
std::shared_ptr<std::thread> thread_;
};
} //namespace lidar
} // namespace livox
#endif // LIVOX_THREAD_BASE_H_
sdk_core/base/wake_up/unix/wake_up_pipe.cpp
+119
View File
@@ -0,0 +1,119 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef WIN32
#include "base/wake_up/wake_up_pipe.h"
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
namespace livox {
namespace lidar {
WakeUpPipe::~WakeUpPipe() {
PipeDestroy();
}
bool WakeUpPipe::WakeUp() {
char ch = '1';
ssize_t nbytes = sizeof(ch);
if (pipe_in_ > 0) {
if (nbytes != write(pipe_in_, &ch, nbytes)) {
return false;
}
}
return true;
}
bool WakeUpPipe::Drain() {
char ch[512];
size_t size = sizeof(ch);
if (pipe_out_ > 0) {
ssize_t ret = read(pipe_out_, ch, size);
if (ret < 0) {
return false;
}
}
return true;
}
bool WakeUpPipe::PipeDestroy() {
if (pipe_in_ > 0) {
close(pipe_in_);
}
if (pipe_out_ > 0) {
close(pipe_out_);
}
return true;
}
bool WakeUpPipe::PipeCreate() {
bool status = false;
//in filedes[0]
//out filedes[1]
int filedes[2]= {};
if (pipe(filedes) == -1) {
return false;
}
do {
int flags = 0;
if ((flags = fcntl(filedes[0], F_GETFD)) == -1) {
break;
}
flags |= FD_CLOEXEC;
if (fcntl(filedes[0], F_SETFD, flags) == -1) {
break;
}
flags = 0;
if ((flags = fcntl(filedes[1], F_GETFD)) == -1) {
break;
}
flags |= FD_CLOEXEC;
if (fcntl(filedes[1], F_SETFD, flags) == -1) {
break;
}
status = true;
} while(0);
if (!status) {
if (filedes[0] > 0) {
close(filedes[0]);
}
if (filedes[1] > 0) {
close(filedes[1]);
}
return false;
}
pipe_out_ = filedes[0];
pipe_in_ = filedes[1];
return true;
}
} // namespace lidar
} // namespace livox
#endif // WIN32
sdk_core/base/wake_up/wake_up_pipe.h
+48
View File
@@ -0,0 +1,48 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef WAKE_UP_PIPE_H_
#define WAKE_UP_PIPE_H_
namespace livox {
namespace lidar {
class WakeUpPipe {
public:
WakeUpPipe(): pipe_in_(0), pipe_out_(0) {}
virtual ~WakeUpPipe();
bool PipeCreate();
bool PipeDestroy();
bool WakeUp();
bool Drain();
int GetPipeOut() { return pipe_out_; }
protected:
int pipe_in_;
int pipe_out_;
};
} // namespace lidar
} // namespace livox
#endif // WAKE_UP_PIPE_H_
sdk_core/base/wake_up/win/wake_up_pipe.cpp
+134
View File
@@ -0,0 +1,134 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifdef WIN32
#include "base/wake_up/wake_up_pipe.h"
#include <fcntl.h>
#include <winsock2.h>
#include <stdio.h>
#pragma comment(lib,"iphlpapi.lib")
#pragma comment(lib,"ws2_32.lib")
namespace livox {
namespace lidar {
WakeUpPipe::~WakeUpPipe() {
PipeDestroy();
}
bool WakeUpPipe::WakeUp() {
char ch = '1';
size_t nbytes = sizeof(ch);
if (pipe_in_ > 0) {
if (nbytes != send(pipe_in_,&ch, nbytes, 0)) {
return false;
}
}
return true;
}
bool WakeUpPipe::Drain() {
char ch[512];
size_t size = sizeof(ch);
if (pipe_out_ > 0) {
recv(pipe_out_, ch, size, 0);
}
return true;
}
bool WakeUpPipe::PipeDestroy() {
if (pipe_in_ > 0) {
closesocket(pipe_in_);
}
if (pipe_out_ > 0) {
closesocket(pipe_out_);
}
return true;
}
bool WakeUpPipe::PipeCreate() {
int listen_sock = -1;
unsigned long on = 1;
bool status = false;
if ((listen_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == INVALID_SOCKET) {
return false;
}
struct sockaddr_in servaddr;
int servaddr_len = sizeof(servaddr);
servaddr.sin_family = AF_INET;
servaddr.sin_port = 0;
servaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
do {
if (bind(listen_sock, (const struct sockaddr *)&servaddr, sizeof(servaddr)) == SOCKET_ERROR) {
break;
}
if (getsockname(listen_sock, (struct sockaddr *)&servaddr, &servaddr_len) == SOCKET_ERROR) {
break;
}
if (listen(listen_sock, 1) == SOCKET_ERROR) {
break;
}
if ((pipe_in_ = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == INVALID_SOCKET) {
break;
}
if (connect(pipe_in_, (const struct sockaddr *)&servaddr, sizeof(servaddr)) == SOCKET_ERROR) {
break;
}
if (ioctlsocket(listen_sock, FIONBIO, &on) == SOCKET_ERROR) {
break;
}
struct sockaddr_in clientaddr;
int clientaddr_len = sizeof(clientaddr);
fd_set poll_set;
FD_ZERO(&poll_set);
FD_SET(listen_sock, &poll_set);
// timeout 2s
struct timeval timeout = {2, 0};
int rv = select(0, &poll_set, nullptr, nullptr, &timeout);
if (rv <= 0) {
break;
}
if ((pipe_out_ = accept(listen_sock, (struct sockaddr *)&clientaddr, &clientaddr_len)) == INVALID_SOCKET) {
break;
}
status = true;
} while(0);
closesocket(listen_sock);
if (!status) {
if (pipe_in_ > 0) {
closesocket(pipe_in_);
}
return false;
}
return true;
}
} // namespace lidar
} // namespace livox
#endif // WIN32
sdk_core/comm/comm_port.cpp
+58
View File
@@ -0,0 +1,58 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "comm/comm_port.h"
#include <stdio.h>
#include <string.h>
#include <iostream>
#include "sdk_protocol.h"
namespace livox {
namespace lidar {
CommPort::CommPort() {
protocol_ = new SdkProtocol();
}
CommPort::~CommPort() {
if (protocol_) {
delete protocol_;
}
}
int32_t CommPort::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet) {
return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet);
}
bool CommPort::ParseCommStream(uint8_t *buf, uint32_t buf_size, CommPacket *o_pack) {
if (!(protocol_->CheckPreamble(buf, buf_size))) {
printf("Comm Port Check Preamble error.\n");
return false;
}
return protocol_->ParsePacket(buf, buf_size, o_pack);
}
} // namespace lidar
} // namespace livox
sdk_core/comm/comm_port.h
+50
View File
@@ -0,0 +1,50 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef COMM_COMM_PORT_H_
#define COMM_COMM_PORT_H_
#include <stdint.h>
#include "protocol.h"
namespace livox {
namespace lidar {
const uint32_t kCacheSize = 8192;
class CommPort {
public:
CommPort();
~CommPort();
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet);
bool ParseCommStream(uint8_t *o_buf, uint32_t buf_size, CommPacket *o_pack);
private:
Protocol *protocol_;
};
} // namespace lidar
} // namespace livox
#endif // COMM_COMM_PORT_H_
sdk_core/comm/define.h
+382
View File
@@ -0,0 +1,382 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_DEFINE_H_
#define LIVOX_DEFINE_H_
#include <stdio.h>
#include <string>
#include <memory>
#include <functional>
#include <vector>
#include <atomic>
#include "livox_lidar_def.h"
namespace livox {
namespace lidar {
#pragma pack(1)
const uint16_t KDefaultTimeOut = 1000;
static const uint32_t kMaxCommandBufferSize = 1400;
typedef struct {
std::string lidar_ipaddr;
std::string lidar_subnet_mask;
std::string lidar_gateway;
uint16_t cmd_data_port;
uint16_t push_msg_port;
uint16_t point_data_port;
uint16_t imu_data_port;
uint16_t log_data_port;
} LivoxLidarNetInfo;
typedef struct {
std::string host_ip;
std::string multicast_ip;
uint16_t cmd_data_port;
uint16_t push_msg_port;
uint16_t point_data_port;
uint16_t imu_data_port;
uint16_t log_data_port;
} HostNetInfo;
typedef struct {
std::vector<uint16_t> cmd_key_set;
} GeneralCfgInfo;
typedef struct {
uint8_t device_type;
LivoxLidarNetInfo lidar_net_info;
HostNetInfo host_net_info;
GeneralCfgInfo general_cfg_info;
} LivoxLidarCfg;
typedef struct {
bool lidar_log_enable;
uint64_t lidar_log_cache_size;
std::string lidar_log_path;
} LivoxLidarLoggerCfg;
typedef struct {
bool master_sdk;
} LivoxLidarSdkFrameworkCfg;
typedef enum {
/**
* Lidar command set, set the working mode and sub working mode of a LiDAR.
*/
kCommandIDLidarSearch = 0x0000,
// kCommandIDLidarPreconfig = 0x01,
kCommandIDLidarWorkModeControl = 0x0100,
kCommandIDLidarGetInternalInfo = 0x0101,
kCommandIDLidarPushMsg = 0x0102,
kCommandIDLidarRebootDevice = 0x0200,
kCommandIDLidarResetDevice = 0x0201,
kCommandIDLidarSetPPSSync = 0x0202,
kCommandIDLidarPushLog = 0x0300,
kCommandIDLidarCollectionLog = 0x0301,
kCommandIDLidarLogSysTimeSync = 0x0302,
kCommandIDLidarDebugPointCloudControl = 0x0303,
kCommandIDGeneralRequestUpgrade = 0x0400,
kCommandIDGeneralXferFirmware = 0x0401,
kCommandIDGeneralCompleteXferFirmware = 0x0402,
kCommandIDGeneralRequestUpgradeProgress = 0x0403,
kCommandIDGeneralRequestFirmwareInfo = 0xFF,
kCommandIDLidarCommandCount
} LidarCommandID;
typedef enum {
/** command type, which requires response from the receiver. */
kCommandTypeCmd = 0,
/** acknowledge type, which is the response of command type. */
kCommandTypeAck = 1,
} CommandType;
typedef enum {
/** command type, which requires response from the receiver. */
kHostSend = 0,
/** acknowledge type, which is the response of command type. */
kLidarSend = 1,
} SendType;
typedef struct {
uint8_t ret_code;
uint8_t dev_type;
char sn[16];
uint8_t lidar_ip[4];
uint16_t cmd_port;
} DetectionData;
typedef struct {
uint32_t handle;
uint16_t cmd_port;
uint8_t dev_type;
std::atomic<bool> is_get={false};
std::atomic<bool> is_set={false};
} ViewDevice;
typedef struct {
uint32_t handle;
uint8_t dev_type;
std::string host_ip;
uint16_t lidar_cmd_port;
uint16_t lidar_point_port;
uint16_t lidar_imu_data_port;
uint16_t host_point_port;
uint16_t host_imu_data_port;
} ViewLidarIpInfo;
typedef struct {
uint8_t lidar_ipaddr[4];
uint8_t lidar_subnet_mask[4];
uint8_t lidar_gateway[4];
} LivoxLidarIpInfoValue;
typedef struct {
uint8_t host_ip[4];
uint16_t host_port;
uint16_t lidar_port;
} HostIpInfoValue;
static const uint16_t kDetectionPort = 56000;
static const uint16_t kDetectionListenPort = 56001;
static const uint16_t kHostDebugPointCloudPort = 44332;
static const uint16_t kHAPCmdPort = 56000;
static const uint16_t kHAPPushMsgPort = 56000;
static const uint16_t kHAPPointDataPort = 57000;
static const uint16_t kHAPIMUPort = 58000;
static const uint16_t kHAPLogPort = 59000;
static const uint16_t kHAPDebugPointCloudPort = 60000;
/** kLogPort, which is the log port to be banned. */
static const uint16_t kLogPort = 0;
static const uint16_t kHAPLidarCmdPort = 56000;
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 kMid360LidarDebugPointCloudPort = 60301;
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;
static const uint16_t kPaHostFaultPort = 42867;
static const uint16_t kMid360sLidarCmdPort = 56100;
static const uint16_t kMid360sLidarPushMsgPort = 56200;
static const uint16_t kMid360sLidarPointCloudPort = 56300;
static const uint16_t kMid360sLidarImuDataPort = 56400;
static const uint16_t kMid360sLidarLogPort = 56500;
static const uint16_t kMid360sLidarDebugPointCloudPort = 60301;
static const uint16_t kMid360sHostCmdPort = 56101;
static const uint16_t kMid360sHostPushMsgPort = 56201;
static const uint16_t kMid360sHostPointCloudPort = 56301;
static const uint16_t kMid360sHostImuDataPort = 56401;
static const uint16_t kMid360sHostLogPort = 56501;
typedef enum {
kCmd = 0,
kPush = 1,
kPointCloud = 2,
kImuData = 3,
kLog = 4,
kFault = 5
} HostSocketType;
typedef enum {
kLidarCmdPort = 56100,
kLidarPushCmdPort = 56200,
kLidarPointDataPort = 56300,
kLidarImuDataPort = 56400,
kLidarLogPort = 56500
} LidarPort;
typedef struct {
uint8_t log_type;
uint8_t enable;
} EnableDeviceLoggerRequest;
typedef struct {
std::string sn;
std::uint8_t dev_type;
std::string lidar_ip;
std::uint16_t cmd_port;
} LidarDeviceInfo;
typedef struct {
uint8_t log_type; // 0
uint8_t file_index; // file index
uint8_t file_num;
uint8_t flag;
uint32_t timestamp;
uint16_t rsvd;
uint32_t trans_index;
uint16_t data_length; // log data length
uint8_t data[1]; //data of log
} DeviceLoggerFilePushRequest;
typedef struct {
uint8_t ret_code; // 0
uint8_t log_type; // file index
uint8_t file_index; // 0 for cfg, 1 for log
uint32_t trans_index; //sequence of trans file
} DeviceLoggerFilePushReponse;
enum class Flag : uint8_t {
kNull,
kCreateFile,
kEndFile,
kTransferData
};
using DataCallback = std::function<void(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket *data, void *client_data)>;
using LidarInfoCallback = std::function<void(const uint32_t, const uint8_t, const char*, void*)>;
typedef struct {
uint8_t firmware_type; /**< firmware type. */
uint8_t encrypt_type; /**< encrypt type. */
uint32_t firmware_length; /**< the length of firmware. */
uint8_t dev_type; /**< the device type of the firmware. */
} LivoxLidarStartUpgradeRequest;
typedef struct {
uint8_t firmware_type; /**< firmware type. */
uint8_t encrypt_type; /**< encrypt type. */
uint32_t firmware_length; /**< the length of firmware. */
uint8_t dev_type; /**< the device type of the firmware. */
uint32_t firmware_version; /**< the version of this firmware. */
uint64_t firmware_buildtime; /**< the buildtime of this firmware. */
uint8_t hw_whitelist[32]; /**< the hardware version list that this firmware can be used for. */
} LivoxLidarStartUpgradeRequestV3;
typedef struct {
uint8_t ret_code; /**< Return code. */
} LivoxLidarStartUpgradeResponse;
typedef struct {
uint32_t offset; /**< Return code. */
uint32_t length; /**< Working state. */
uint8_t encrypt_type;
uint8_t rsvd[3];
uint8_t data[1]; /**< LiDAR feature. */
} LivoxLidarXferFirmwareResquest;
typedef struct {
uint8_t ret_code; /**< Return code. */
uint32_t offset; /**< Return code. */
uint32_t length; /**< Working state. */
} LivoxLidarXferFirmwareResponse;
typedef struct {
uint8_t checksum_type; /**< Return code. */
uint8_t checksum_length; /**< Working state. */
uint8_t checksum[1]; /**< LiDAR feature. */
} LivoxLidarCompleteXferFirmwareResquest;
typedef struct {
uint8_t ret_code; /**< Return code. */
} LivoxLidarCompleteXferFirmwareResponse;
typedef struct {
uint8_t ret_code; /**< Return code. */
uint8_t progress; /**< progress of upgrade. */
} LivoxLidarGetUpgradeProgressResponse;
typedef struct {
uint8_t ret_code; /**< Return code. */
uint16_t length; /**< The length of firmware info string, include '\0'. */
uint8_t info[1]; /**< Firmware info string, include '\0'. */
} LivoxLidarRequestFirmwareInfoResponse;
typedef struct {
uint8_t file_ver; /**< file format version. */
uint8_t dev_type; /**< the device type of the firmware. */
uint8_t data_type; /**< type of data. */
uint8_t sn[16];
uint8_t rsvd[107];
uint16_t crc16;
} LivoxLidarDebugPointCloudFileHeader;
typedef struct {
uint8_t enable;
uint8_t host_ip_addr[4];
uint16_t host_port;
uint16_t bandwidth;
} LivoxLidarDebugPointCloudRequest;
typedef struct {
enum class SyncTimeType : std::uint8_t {
kRmcSyncTime = 2,
} type;
uint64_t ns;
} LivoxLidarRmcSyncTimeRequest;
typedef void(*LivoxLidarStartUpgradeCallback)(livox_status status, uint32_t handle,
LivoxLidarStartUpgradeResponse* response, void* client_data);
typedef void(*LivoxLidarXferFirmwareCallback)(livox_status status, uint32_t handle,
LivoxLidarXferFirmwareResponse* response, void* client_data);
typedef void(*LivoxLidarCompleteXferFirmwareCallback)(livox_status status, uint32_t handle,
LivoxLidarCompleteXferFirmwareResponse* response, void* client_data);
typedef void(*LivoxLidarGetUpgradeProgressCallback)(livox_status status,
uint32_t handle, LivoxLidarGetUpgradeProgressResponse* response, void* client_data);
typedef void(*LivoxLidarRequestFirmwareInfoCallback)(livox_status status,
uint32_t handle, LivoxLidarRequestFirmwareInfoResponse* response, void* client_data);
#pragma pack()
} // namespace lidar
} // namespace livox
# endif // DEFINE_H_
sdk_core/comm/generate_seq.cpp
+47
View File
@@ -0,0 +1,47 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "generate_seq.h"
#include <cstdint>
namespace livox {
namespace lidar {
uint32_t GenerateSeq::GetSeq() {
static std::atomic<std::uint32_t> seq(1);
uint32_t value = seq.load();
uint32_t desired = 0;
do {
if (value == UINT16_MAX) {
desired = 1;
} else {
desired = value + 1;
}
} while (!seq.compare_exchange_weak(value, desired));
return desired;
}
} // namespace lidar
} // namespace livox
sdk_core/comm/generate_seq.h
+45
View File
@@ -0,0 +1,45 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_GENERATE_SEQ_H_
#define LIVOX_GENERATE_SEQ_H_
#include <cstdint>
#include <memory>
#include <atomic>
namespace livox {
namespace lidar {
class GenerateSeq {
public:
static uint32_t GetSeq();
};
} // namespace lidar
} // namespace livox
#endif // GENERATE_SEQ_H_
sdk_core/comm/protocol.h
+92
View File
@@ -0,0 +1,92 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef COMM_PROTOCOL_H_
#define COMM_PROTOCOL_H_
#include <stdint.h>
namespace livox {
namespace lidar {
typedef struct CommPacket CommPacket;
typedef int (*RequestPackCb)(CommPacket *packet);
typedef enum { kRequestPack, kAckPack, kMsgPack } PacketType;
typedef enum { kLidarSdk, kRsvd1, kProtocolUndef } ProtocolType;
typedef enum { kNoNeed, kNeedAck, kDelayAck } NeedAckType;
typedef enum { kParseSuccess, kParseFail } ParseResult;
typedef struct LogCommPacket {
uint8_t packet_type;
uint8_t protocol;
uint8_t protocol_version;
uint8_t cmd_set;
uint32_t cmd_code;
uint32_t sender;
uint32_t sub_sender;
uint32_t receiver;
uint32_t sub_receiver;
uint32_t seq_num;
uint8_t *data;
uint16_t data_len;
uint32_t padding;
} LogCommPacket;
typedef struct CommPacket {
uint8_t protocol;
uint8_t version;
uint32_t seq_num;
uint16_t cmd_id;
uint8_t cmd_type;
uint8_t sender_type;
uint8_t* data;
uint16_t data_len;
} CommPacket;
class Protocol {
public:
virtual ~Protocol(){};
virtual bool ParsePacket(uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) = 0;
virtual int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet) = 0;
virtual uint32_t GetPreambleLen() = 0;
virtual uint32_t GetPacketWrapperLen() = 0;
virtual uint32_t GetPacketLen(uint8_t *buf) = 0;
virtual bool CheckPreamble(uint8_t *buf, uint32_t buf_size) = 0;
};
} // namespace lidar
} // namespace livox
#endif // COMM_PROTOCOL_H_
sdk_core/comm/sdk_protocol.cpp
+148
View File
@@ -0,0 +1,148 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "sdk_protocol.h"
#include <stdio.h>
#include <string.h>
namespace livox {
namespace lidar {
const uint8_t kSdkProtocolSof = 0xAA;
const uint8_t kSdkVer = 0;
const uint32_t kSdkPacketCrcSize = 4; // crc32
const uint32_t kSdkPacketPreambleCrcSize = 2; // crc16
SdkProtocol::SdkProtocol() {}
SdkProtocol::~SdkProtocol() {
}
int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet) {
if (kLidarSdk != i_packet.protocol) {
return -1;
}
SdkPacket *sdk_packet = (SdkPacket *)o_buf;
sdk_packet->sof = kSdkProtocolSof;
sdk_packet->version = kSdkVer;
sdk_packet->length = i_packet.data_len + GetPacketWrapperLen();
if (sdk_packet->length > o_buf_size) {
return -1;
}
sdk_packet->seq_num = i_packet.seq_num & 0xFFFF;
sdk_packet->cmd_id = i_packet.cmd_id;
sdk_packet->cmd_type = i_packet.cmd_type;
sdk_packet->sender_type = i_packet.sender_type;
sdk_packet->crc16_h = crc_16_.ccitt(o_buf, 18);
if (i_packet.data_len == 0) {
sdk_packet->crc32_d = 0;
} else {
sdk_packet->crc32_d = crc_32_.crc32(i_packet.data, i_packet.data_len);
}
memcpy(sdk_packet->data, i_packet.data, i_packet.data_len);
*o_len = sdk_packet->length;
return 0;
}
bool SdkProtocol::ParsePacket(uint8_t *i_buf, uint32_t buf_size, CommPacket *o_packet) {
SdkPacket *sdk_packet = (SdkPacket *)i_buf;
if (buf_size < GetPacketWrapperLen()) {
return false;
}
memset((void *)o_packet, 0, sizeof(CommPacket));
o_packet->protocol = kLidarSdk;
o_packet->version = sdk_packet->version;
o_packet->seq_num = sdk_packet->seq_num;
o_packet->cmd_id = sdk_packet->cmd_id;
o_packet->cmd_type = sdk_packet->cmd_type;
o_packet->sender_type = sdk_packet->sender_type;
o_packet->data = sdk_packet->data;
o_packet->data_len = sdk_packet->length - GetPacketWrapperLen();
return true;
}
uint32_t SdkProtocol::GetPreambleLen() {
return sizeof(SdkPreamble);
}
uint32_t SdkProtocol::GetPacketWrapperLen() {
return sizeof(SdkPacket) - 1;
}
uint32_t SdkProtocol::GetPacketLen(uint8_t *buf) {
SdkPreamble *preamble = (SdkPreamble *)buf;
return preamble->length;
}
bool SdkProtocol::CheckPreamble(uint8_t *buf, uint32_t buf_size) {
if (buf_size < GetPreambleLen()) {
return false;
}
SdkPacket *packet = (SdkPacket *)buf;
if (packet->sof != kSdkProtocolSof) {
return false;
}
if (packet->version != kSdkVer) {
return false;
}
if (packet->length < GetPreambleLen()) {
return false;
}
uint16_t crc16_h = crc_16_.ccitt(buf, 18);
if (packet->crc16_h != crc16_h) {
return false;
}
uint32_t crc32_d = 0;
if (packet->length - GetPacketWrapperLen() == 0) {
crc32_d = 0;
} else {
crc32_d = crc_32_.crc32(packet->data, packet->length - GetPacketWrapperLen());
}
if (packet->crc32_d != crc32_d) {
return false;
}
return true;
}
} // namespace lidar
} // namespace livox
sdk_core/comm/sdk_protocol.h
+91
View File
@@ -0,0 +1,91 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_SDK_PROTOCOL_H_
#define LIVOX_SDK_PROTOCOL_H_
#include <stdint.h>
#include "comm/protocol.h"
#include "FastCRC/FastCRC.h"
namespace livox {
namespace lidar {
typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion;
#pragma pack(1)
typedef struct {
uint8_t sof;
uint8_t version;
uint16_t length;
uint32_t seq_num;
uint16_t cmd_id;
uint8_t cmd_type;
uint8_t sender_type;
char rsvd[6];
uint16_t crc16_h;
uint32_t crc32_d;
} SdkPreamble;
typedef struct {
uint8_t sof;
uint8_t version;
uint16_t length;
uint32_t seq_num;
uint16_t cmd_id;
uint8_t cmd_type;
uint8_t sender_type;
char rsvd[6];
uint16_t crc16_h;
uint32_t crc32_d;
uint8_t data[1];
} SdkPacket;
#pragma pack()
class SdkProtocol : public Protocol {
public:
SdkProtocol();
~SdkProtocol();
bool ParsePacket(uint8_t *i_buf, uint32_t buf_size, CommPacket *o_packet);
int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet);
uint32_t GetPreambleLen();
uint32_t GetPacketWrapperLen();
uint32_t GetPacketLen(uint8_t *buf);
bool CheckPreamble(uint8_t *buf, uint32_t buf_size);
private:
FastCRC16 crc_16_;
FastCRC32 crc_32_;
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_SDK_PROTOCOL_H_
sdk_core/command_handler/build_request.cpp
+431
View File
@@ -0,0 +1,431 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "build_request.h"
#include <stdio.h>
#include <functional>
#include <atomic>
#include <memory>
#include "base/logging.h"
#include "comm/define.h"
namespace livox {
namespace lidar {
bool BuildRequest::BuildUpdateViewLidarCfgRequest(const ViewLidarIpInfo& view_lidar_info, uint8_t* req_buf, uint16_t& req_len) {
uint16_t key_num = 0;
if(view_lidar_info.dev_type == kLivoxLidarTypePA) {
key_num = 1;
} else {
key_num = 2;
}
req_len = 0;
memcpy(&req_buf[req_len], &key_num, sizeof(key_num));
req_len = sizeof(key_num) + sizeof(uint16_t);
LivoxLidarKeyValueParam * point_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
point_kv->key = static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg);
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;
}
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);
if (view_lidar_info.dev_type == kLivoxLidarTypePA) {
return true;
}
LivoxLidarKeyValueParam * imu_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
imu_kv->key = static_cast<uint16_t>(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;
}
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<uint16_t>(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.host_ip, host_state_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init host ip addr failed.");
return false;
}
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_state_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init host ip addr failed.");
return false;
}
} else {
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_state_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init pointcloud multicast 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<uint16_t>(kKeyLidarPointDataHostIpCfg);
point_kv->length = sizeof(uint8_t) * 8;
HostIpInfoValue* host_point_ip_info_val = (HostIpInfoValue*)&point_kv->value;
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_point_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init pointcloud host ip addr failed.");
return false;
}
} else {
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_point_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init pointcloud multicast 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<uint16_t>(kKeyLidarImuHostIpCfg);
imu_kv->length = sizeof(uint8_t) * 8;
HostIpInfoValue* host_imu_ip_info_val = (HostIpInfoValue*)&imu_kv->value;
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_imu_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init imu host ip addr failed.");
return false;
}
} else {
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_imu_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init imu multicast 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 = 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);
LivoxLidarKeyValueParam * point_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
point_kv->key = static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg);
point_kv->length = sizeof(uint8_t) * 8;
HostIpInfoValue* host_point_ip_info_val = (HostIpInfoValue*)&point_kv->value;
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_point_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init pointcloud host ip addr failed.");
return false;
}
} else {
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_point_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init pointcloud multicast ip addr failed.");
return false;
}
}
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 if (lidar_cfg.device_type == kLivoxLidarTypeMid360s) {
lidar_point_port = kMid360sLidarPointCloudPort;
}
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<uint16_t>(kKeyLidarImuHostIpCfg);
imu_kv->length = sizeof(uint8_t) * 8;
HostIpInfoValue* host_imu_ip_info_val = (HostIpInfoValue*)&imu_kv->value;
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_imu_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init imu host ip addr failed.");
return false;
}
} else {
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_imu_ip_info_val)) {
LOG_ERROR("Build update lidar cfg request failed, init imu multicast ip addr failed.");
return false;
}
}
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 if (lidar_cfg.device_type == kLivoxLidarTypeMid360s) {
lidar_imu_port = kMid360sLidarImuDataPort;
}
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;
}
bool BuildRequest::BuildSetLidarIPInfoRequest(const LivoxLidarIpInfo& lidar_ip_config, 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<uint16_t>(kKeyLidarIpCfg);
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;
}
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(LivoxLidarIpInfoValue);
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<uint16_t>(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));
req_len = sizeof(key_num) + sizeof(uint16_t);
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
kv->key = static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg);
kv->length = sizeof(uint8_t) * 8;
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;
}
memcpy(&(host_ip_info_val->host_port), &host_point_ip_cfg.host_point_data_port, sizeof(host_point_ip_cfg.host_point_data_port));
memcpy(&(host_ip_info_val->lidar_port), &host_point_ip_cfg.lidar_point_data_port, sizeof(host_point_ip_cfg.lidar_point_data_port));
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
return true;
}
bool BuildRequest::BuildSetHostImuDataIPInfoRequest(const HostImuDataIPInfo& host_imu_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<uint16_t>(kKeyLidarImuHostIpCfg);
kv->length = sizeof(uint8_t) * 8;
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;
}
memcpy(&(host_ip_info_val->host_port), &host_imu_ipcfg.host_imu_data_port, sizeof(host_imu_ipcfg.host_imu_data_port));
memcpy(&(host_ip_info_val->lidar_port), &host_imu_ipcfg.lidar_imu_data_port, sizeof(host_imu_ipcfg.lidar_imu_data_port));
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
return true;
}
bool BuildRequest::InitLidarIpinfoVal(const LivoxLidarIpInfo& lidar_ip_config, LivoxLidarIpInfoValue* lidar_ipinfo_val_ptr) {
if (lidar_ipinfo_val_ptr == nullptr) {
return false;
}
// Set lidar ip info
std::vector<uint8_t> vec_lidar_ip;
if (!IpToU8(lidar_ip_config.ip_addr, ".", vec_lidar_ip)) {
return false;
}
memcpy(lidar_ipinfo_val_ptr->lidar_ipaddr, vec_lidar_ip.data(), sizeof(uint8_t) * 4);
// Set lidar subnet mask
std::vector<uint8_t> vec_lidar_subnet_mask;
if (!IpToU8(lidar_ip_config.net_mask, ".", vec_lidar_subnet_mask)) {
return false;
}
memcpy(lidar_ipinfo_val_ptr->lidar_subnet_mask, vec_lidar_subnet_mask.data(), sizeof(uint8_t) * 4);
// Set lidar gateway
std::vector<uint8_t> vec_lidar_gateway;
if (!IpToU8(lidar_ip_config.gw_addr, ".", vec_lidar_gateway)) {
return false;
}
memcpy(lidar_ipinfo_val_ptr->lidar_gateway, vec_lidar_gateway.data(), sizeof(uint8_t) * 4);
return true;
}
bool BuildRequest::InitHostIpAddr(const std::string& host_ip, HostIpInfoValue* host_ipinfo_val_ptr) {
if (host_ipinfo_val_ptr == nullptr) {
return false;
}
// Set lidar ip info
std::vector<uint8_t> vec_host_ip;
if (!IpToU8(host_ip, ".", vec_host_ip)) {
return false;
}
memcpy(host_ipinfo_val_ptr->host_ip, vec_host_ip.data(), sizeof(uint8_t) * 4);
return true;
}
bool BuildRequest::InitMulticastHostIpAddr(const std::string& multicast_ip, HostIpInfoValue* host_ipinfo_val_ptr) {
if (host_ipinfo_val_ptr == nullptr) {
return false;
}
// Set lidar multicast ip info
std::vector<uint8_t> vec_host_ip;
if (!IpToU8(multicast_ip, ".", vec_host_ip)) {
return false;
}
memcpy(host_ipinfo_val_ptr->host_ip, vec_host_ip.data(), sizeof(uint8_t) * 4);
return true;
}
bool BuildRequest::IpToU8(const std::string& src, const std::string& seq, std::vector<uint8_t>& result) {
std::string::size_type pos1, pos2;
pos2 = src.find(seq);
pos1 = 0;
while (std::string::npos != pos2) {
int32_t val = std::stoi(src.substr(pos1, pos2-pos1));
if (val < 0 || val > 256) {
LOG_ERROR("Build broadcast request failed, ip to u8 failed, the ip:{}, the fault val:{}",
src, val);
return false;
}
result.push_back(val);
pos1 = pos2 + seq.size();
pos2 = src.find(seq, pos1);
}
if (pos1 != src.length()) {
int32_t val = std::stoi(src.substr(pos1, pos2-pos1));
if (val < 0 || val > 256) {
LOG_ERROR("Build broadcast request failed, ip to u8 failed, the ip:{}, the fault val:{}",
src, val);
return false;
}
result.push_back(val);
}
if (result.size() != 4) {
LOG_ERROR("Build broadcast request failed, ip to u8 failed, the ip:{}, the val size:{}",
src.c_str(), result.size());
return false;
}
return true;
}
} // namespace lidar
} // namespace livox
sdk_core/command_handler/build_request.h
+66
View File
@@ -0,0 +1,66 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_BUILD_REQUEST_H_
#define LIVOX_BUILD_REQUEST_H_
#include <memory>
#include <map>
#include <list>
#include <string>
#include <algorithm>
#include <string.h>
#include "base/io_loop.h"
#include "comm/comm_port.h"
#include "livox_lidar_def.h"
#include "comm/define.h"
#include <memory>
namespace livox {
namespace lidar {
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<uint8_t>& result);
private:
static bool InitLidarIpinfoVal(const LivoxLidarIpInfo& lidar_ip_config, LivoxLidarIpInfoValue* lidar_ipinfo_val_ptr);
static bool InitMulticastHostIpAddr(const std::string& multicast_ip, HostIpInfoValue* host_ipinfo_val_ptr);
static bool InitHostIpAddr(const std::string& host_ip, HostIpInfoValue* host_ipinfo_val_ptr);
};
} // namespace direct
} // namespace livox
# endif // LIVOX_BUILD_REQUEST_H_
sdk_core/command_handler/command_handler.h
+64
View File
@@ -0,0 +1,64 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef COMMAND_HANDLER_H_
#define COMMAND_HANDLER_H_
#include <memory>
#include <map>
#include <mutex>
#include "base/command_callback.h"
#include "base/io_thread.h"
#include "comm/protocol.h"
#include "comm/define.h"
#include "livox_lidar_def.h"
#include "device_manager.h"
namespace livox {
namespace lidar {
class CommandHandler {
public:
CommandHandler(DeviceManager* device_manager) : device_manager_(device_manager) {}
~CommandHandler() {}
virtual bool Init(bool is_view) = 0;
virtual bool Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map) = 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;
virtual livox_status SendLoggerCommand(const Command &command) = 0;
protected:
DeviceManager* device_manager_;
};
} // namespace livox
} // namespace lidar
#endif // COMMAND_HANDLER_H_
sdk_core/command_handler/command_impl.cpp
+886
View File
@@ -0,0 +1,886 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "command_impl.h"
#include "livox_lidar_def.h"
#include "general_command_handler.h"
#include "debug_point_cloud_handler/debug_point_cloud_manager.h"
#include "spdlog/fmt/fmt.h"
#include "base/logging.h"
#include "comm/protocol.h"
#include "comm/generate_seq.h"
#include "build_request.h"
#include <sstream>
#include <inttypes.h>
#include <string>
#include <iomanip>
#include <chrono>
#include <vector>
namespace livox {
namespace lidar {
std::int64_t StringToTimestamp(std::string const& fmt, std::string const& date) {
std::tm timestamp = {};
std::stringstream date_ss(date);
date_ss >> std::get_time(&timestamp, fmt.c_str());
auto time_point = std::chrono::system_clock::from_time_t(std::mktime(&timestamp));
return std::chrono::duration_cast<std::chrono::seconds>(time_point.time_since_epoch()).count();
}
std::vector<std::string> Split(std::string const& str, char const pattern) {
std::vector<std::string> res;
std::stringstream input(str);
std::string part;
while (getline(input, part, pattern)) {
res.push_back(part);
}
return res;
}
std::uint64_t ParseGPRMC(std::string const& gprmc) {
std::vector<std::string> gprmc_vec = Split(gprmc, ',');
if (gprmc_vec.size() < 9 || gprmc_vec[1].length() < 6 || gprmc_vec[9].length() < 6) {
LOG_ERROR("gprmc check failed. gprmc is : {}", gprmc);
return 0;
}
auto year = gprmc_vec[9].substr(4);
auto month = gprmc_vec[9].substr(2, 2);
auto day = gprmc_vec[9].substr(0, 2);
auto hour = gprmc_vec[1].substr(0, 2);
auto minute = gprmc_vec[1].substr(2, 2);
auto second = gprmc_vec[1].substr(4, 2);
std::string time = fmt::format("{}-{}-{} {}:{}:{}", "20" + year, month, day, hour, minute, second);
std::uint64_t time_ms = StringToTimestamp("%Y-%m-%d %H:%M:%S", time) * 1000;
return time_ms * 1000 * 1000;
}
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<ParamKeyName> 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<uint16_t>(key);
req_len += sizeof(uint16_t);
}
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarGetInternalInfo,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarDiagInternalInfoResponse>(cb, client_data));
}
livox_status CommandImpl::QueryLivoxLidarFwType(uint32_t handle, QueryLivoxLidarInternalInfoCallback 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* kList = (LivoxLidarKeyValueParam*)&req_buff[req_len];
kList->key = static_cast<uint16_t>(kKeyFwType);
req_len += sizeof(uint16_t);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarGetInternalInfo,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarDiagInternalInfoResponse>(cb, client_data));
}
livox_status CommandImpl::QueryLivoxLidarFirmwareVer(uint32_t handle, QueryLivoxLidarInternalInfoCallback 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* kList = (LivoxLidarKeyValueParam*)&req_buff[req_len];
kList->key = static_cast<uint16_t>(kKeyVersionApp);
req_len += sizeof(uint16_t);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarGetInternalInfo,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarDiagInternalInfoResponse>(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<uint16_t>(kKeyPclDataType);
kv->length = sizeof(uint8_t);
kv->value[0] = static_cast<uint8_t>(data_type);
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(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<uint16_t>(kKeyPatternMode);
kv->length = sizeof(uint8_t);
kv->value[0] = static_cast<uint8_t>(scan_pattern);
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(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<uint16_t>(kKeyDualEmitEn);
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<LivoxLidarAsyncControlResponse>(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<uint16_t>(kKeyPointSendEn);
kv->length = sizeof(uint8_t);
kv->value[0] = 0x00;
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(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<uint16_t>(kKeyPointSendEn);
kv->length = sizeof(uint8_t);
kv->value[0] = 0x01;
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(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<LivoxLidarAsyncControlResponse>(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<LivoxLidarAsyncControlResponse>(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<LivoxLidarAsyncControlResponse>(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<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(cb, client_data));
}
livox_status CommandImpl::SetLivoxLidarFovCfg0(uint32_t handle, const FovCfg& fov_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<uint16_t>(kKeyFovCfg0);
kv->length = sizeof(FovCfg);
FovCfg* fov_cfg = (FovCfg*)&kv->value;
memcpy(fov_cfg, &fov_cfg0, sizeof(FovCfg));
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(FovCfg);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
}
livox_status CommandImpl::SetLivoxLidarFovCfg1(uint32_t handle, const FovCfg& fov_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<uint16_t>(kKeyFovCfg1);
kv->length = sizeof(FovCfg);
FovCfg* fov_cfg = (FovCfg*)&kv->value;
memcpy(fov_cfg, &fov_cfg1, sizeof(FovCfg));
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(FovCfg);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
}
livox_status CommandImpl::EnableLivoxLidarFov(uint32_t handle, uint8_t fov_en, 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<uint16_t>(kKeyFovCfgEn);
kv->length = sizeof(uint8_t);
kv->value[0] = fov_en;
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
}
livox_status CommandImpl::DisableLivoxLidarFov(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<uint16_t>(kKeyFovCfgEn);
kv->length = sizeof(uint8_t);
kv->value[0] = 0x00;
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(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<uint16_t>(kKeyDetectMode);
kv->length = sizeof(uint8_t);
kv->value[0] = static_cast<uint8_t>(mode);
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(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<uint16_t>(kKeyBlindSpotSet);
kv->length = sizeof(uint32_t);
uint32_t* blind_spot_set = reinterpret_cast<uint32_t*>(&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<LivoxLidarAsyncControlResponse>(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<uint16_t>(kKeyWorkMode);
kv->length = sizeof(uint8_t);
uint8_t* val_work_mode = reinterpret_cast<uint8_t*>(&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<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(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<uint16_t>(kKeyGlassHeat);
kv->length = sizeof(uint8_t);
kv->value[0] = static_cast<uint8_t>(glass_heat);
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(cb, client_data));
}
livox_status CommandImpl::StartForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return SendSingleControlCommand(handle, cb, client_data, kKeyForceHeatEn, 0x01/*enable forced heating*/);
}
livox_status CommandImpl::StopForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return SendSingleControlCommand(handle, cb, client_data, kKeyForceHeatEn, 0x00/*disable forced heating*/);
}
livox_status CommandImpl::SetLivoxLidarEscMode(uint32_t handle, LivoxLidarEscMode esc_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<uint16_t>(kKeySetEscMode);
kv->length = sizeof(uint8_t);
uint8_t* val_esc_mode = reinterpret_cast<uint8_t*>(&kv->value[0]);
*val_esc_mode = esc_mode;
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint8_t);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(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<uint16_t>(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<LivoxLidarAsyncControlResponse>(cb, client_data));
}
livox_status CommandImpl::LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) {
return GeneralCommandHandler::GetInstance().LivoxLidarRequestReset(handle, cb, client_data);
}
livox_status CommandImpl::SetLivoxLidarDebugPointCloud(uint32_t handle, bool enable,
LivoxLidarLoggerCallback cb, void* client_data) {
DebugPointCloudManager::GetInstance().Enable(enable);
LivoxLidarDebugPointCloudRequest req_buff {};
req_buff.enable = enable ? 1 : 0;
req_buff.host_port = kHostDebugPointCloudPort; // 44332
req_buff.bandwidth = 0; // units Mbps
sscanf(GeneralCommandHandler::GetInstance().GetLidarCfg(handle).host_net_info.host_ip.c_str(),
"%" SCNu8 ".%" SCNu8 ".%" SCNu8 ".%" SCNu8, &req_buff.host_ip_addr[0]
, &req_buff.host_ip_addr[1]
, &req_buff.host_ip_addr[2]
, &req_buff.host_ip_addr[3]);
return GeneralCommandHandler::GetInstance().SendLoggerCommand(handle,
kCommandIDLidarDebugPointCloudControl,
reinterpret_cast<uint8_t*>(&req_buff),
uint16_t(sizeof(LivoxLidarDebugPointCloudRequest)),
MakeCommandCallback<LivoxLidarLoggerResponse>(cb, client_data));
}
livox_status CommandImpl::SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t rmc_length,
LivoxLidarRmcSyncTimeCallBack cb, void* client_data) {
LivoxLidarRmcSyncTimeRequest req_buff {};
req_buff.type = LivoxLidarRmcSyncTimeRequest::SyncTimeType::kRmcSyncTime;
req_buff.ns = ParseGPRMC(std::string(rmc, rmc_length));
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarSetPPSSync,
reinterpret_cast<uint8_t*>(&req_buff),
uint16_t(sizeof(LivoxLidarRmcSyncTimeRequest)),
MakeCommandCallback<LivoxLidarRmcSyncTimeResponse>(cb, client_data));
}
livox_status CommandImpl::SetLivoxLidarWorkModeAfterBoot(uint32_t handle, LivoxLidarWorkModeAfterBoot 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<uint16_t>(kKeyWorkModeAfterBoot);
kv->length = sizeof(uint8_t);
uint8_t* val_work_mode = reinterpret_cast<uint8_t*>(&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<LivoxLidarAsyncControlResponse>(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<LivoxLidarStartUpgradeResponse>(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<LivoxLidarXferFirmwareResponse>(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<LivoxLidarCompleteXferFirmwareResponse>(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<LivoxLidarGetUpgradeProgressResponse>(cb, client_data));
}
livox_status CommandImpl::LivoxLidarRequestFirmwareInfo(uint32_t handle,
LivoxLidarRequestFirmwareInfoCallback cb, void* client_data) {
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDGeneralRequestFirmwareInfo,
nullptr,
0,
MakeCommandCallback<LivoxLidarRequestFirmwareInfoResponse>(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<LivoxLidarRebootResponse>(cb,
client_data));
}
livox_status CommandImpl::SendSingleControlCommand(uint32_t handle,
LivoxLidarAsyncControlCallback cb,
void* client_data,
uint16_t command_key,
uint8_t value) {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t key_num = 1;
uint16_t req_len = 0;
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 = command_key;
kv->length = sizeof(uint8_t);
kv->value[0] = value;
req_len += sizeof(LivoxLidarKeyValueParam);
return GeneralCommandHandler::GetInstance().SendCommand(handle,
kCommandIDLidarWorkModeControl,
req_buff,
req_len,
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
}
} // namespace livox
} // namespace lidar
sdk_core/command_handler/command_impl.h
+149
View File
@@ -0,0 +1,149 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef COMMAND_IMPL_H_
#define COMMAND_IMPL_H_
#include <memory>
#include <map>
#include <mutex>
#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 QueryLivoxLidarFwType(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data);
static livox_status QueryLivoxLidarFirmwareVer(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 SetLivoxLidarFovCfg0(uint32_t handle, const FovCfg& fov_cfg0, LivoxLidarAsyncControlCallback cb, void* client_data);
static livox_status SetLivoxLidarFovCfg1(uint32_t handle, const FovCfg& fov_cfg1, LivoxLidarAsyncControlCallback cb, void* client_data);
static livox_status EnableLivoxLidarFov(uint32_t handle, uint8_t fov_en, LivoxLidarAsyncControlCallback cb, void* client_data);
static livox_status DisableLivoxLidarFov(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 StartForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
static livox_status StopForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
static livox_status SetLivoxLidarEscMode(uint32_t handle, LivoxLidarEscMode esc_mode, 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);
static livox_status SetLivoxLidarDebugPointCloud(uint32_t handle, bool enable, LivoxLidarLoggerCallback cb, void* client_data);
static livox_status SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t rmc_length, LivoxLidarRmcSyncTimeCallBack cb, void* client_data);
static livox_status SetLivoxLidarWorkModeAfterBoot(uint32_t handle, LivoxLidarWorkModeAfterBoot work_mode, LivoxLidarAsyncControlCallback 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);
private:
static livox_status SendSingleControlCommand(uint32_t handle,
LivoxLidarAsyncControlCallback cb,
void* client_data,
uint16_t command_key,
uint8_t value);
};
} // namespace livox
} // namespace lidar
#endif // COMMAND_IMPL_H_
sdk_core/command_handler/general_command_handler.cpp
+813
View File
@@ -0,0 +1,813 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "general_command_handler.h"
#include <iostream>
#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/mid360s_command_handler.h"
#include "logger_handler/logger_manager.h"
#include "debug_point_cloud_handler/debug_point_cloud_manager.h"
#include "base/logging.h"
#include "comm/protocol.h"
#include "comm/generate_seq.h"
#include "build_request.h"
namespace livox {
namespace lidar {
GeneralCommandHandler::GeneralCommandHandler()
: device_manager_(nullptr),
comm_port_(nullptr),
livox_lidar_info_change_cb_(nullptr),
livox_lidar_info_change_client_data_(nullptr),
livox_lidar_info_cb_(nullptr),
livox_lidar_info_client_data_(nullptr),
detection_host_ip_(""),
is_view_(false) {
}
GeneralCommandHandler& GeneralCommandHandler::GetInstance() {
static GeneralCommandHandler general_command_handler;
return general_command_handler;
}
bool GeneralCommandHandler::Init(const std::string& host_ip, const bool is_view, DeviceManager* device_manager) {
is_view_ = is_view;
detection_host_ip_ = host_ip;
device_manager_ = device_manager;
comm_port_.reset(new CommPort());
return true;
}
bool GeneralCommandHandler::Init(std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr, DeviceManager* device_manager) {
if (comm_port_ == nullptr) {
is_view_ = false;
device_manager_ = device_manager;
comm_port_.reset(new CommPort());
}
if (lidars_command_handler_.find(kLivoxLidarTypeIndustrialHAP) == lidars_command_handler_.end()) {
lidars_command_handler_[kLivoxLidarTypeIndustrialHAP].reset(new HapCommandHandler(device_manager_));
}
if (lidars_command_handler_.find(kLivoxLidarTypeMid360) == lidars_command_handler_.end()) {
lidars_command_handler_[kLivoxLidarTypeMid360].reset(new Mid360CommandHandler(device_manager_));
}
if (lidars_command_handler_.find(kLivoxLidarTypeMid360s) == lidars_command_handler_.end()) {
lidars_command_handler_[kLivoxLidarTypeMid360s].reset(new Mid360sCommandHandler(device_manager_));
}
AddDetectedLidar(custom_lidars_cfg_ptr);
return true;
}
void GeneralCommandHandler::AddDetectedLidar(const std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr) {
for (auto it = custom_lidars_cfg_ptr->begin(); it != custom_lidars_cfg_ptr->end(); ++it) {
const LivoxLidarCfg& lidar_cfg = *it;
uint32_t lidar_ip = inet_addr(lidar_cfg.lidar_net_info.lidar_ipaddr.c_str());
if (custom_lidars_cfg_map_.find(lidar_ip) == custom_lidars_cfg_map_.end()) {
custom_lidars_cfg_map_[lidar_ip] = lidar_cfg;
}
}
}
void GeneralCommandHandler::Destory() {
device_manager_ = nullptr;
comm_port_.reset(nullptr);
{
std::lock_guard<std::mutex> lock(dev_type_mutex_);
device_dev_type_.clear();
}
{
std::lock_guard<std::mutex> lock(devices_mutex_);
devices_.clear();
}
{
std::lock_guard<std::mutex> lock(command_handle_mutex_);
lidars_command_handler_.clear();
}
{
std::mutex commands_mutex_;
std::map<uint32_t, std::pair<Command, TimePoint> > commands_;
}
livox_lidar_info_change_cb_ = nullptr;
livox_lidar_info_change_client_data_ = nullptr;
detection_host_ip_ = "";
is_view_ = false;
}
GeneralCommandHandler::~GeneralCommandHandler() {
Destory();
}
void GeneralCommandHandler::Handler(uint32_t handle, uint16_t lidar_port, uint8_t *buf, uint32_t buf_size) {
if (buf == nullptr || buf_size == 0) {
return;
}
CommPacket packet;
memset(&packet, 0, sizeof(packet));
if (!(comm_port_->ParseCommStream((uint8_t*)buf, buf_size, &packet))) {
LOG_INFO("Parse GeneralCommandHandler Command Stream failed.");
return;
}
if (lidar_port == kDetectionPort && packet.cmd_id == kCommandIDLidarSearch) {
if (packet.cmd_type == kCommandTypeCmd) {
return;
}
HandleDetectionData(handle, lidar_port, packet);
return;
}
if (packet.cmd_type == kCommandTypeAck) {
uint32_t seq = packet.seq_num;
Command command;
{
std::lock_guard<std::mutex> lock(commands_mutex_);
if (commands_.find(seq) == commands_.end()) {
LOG_ERROR("Handle cmd ack failed, can not find command");
return;
}
command = commands_[seq].first;
command.packet = packet;
commands_.erase(seq);
}
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data);
}
return;
}
if (packet.cmd_id == kCommandIDLidarPushMsg) {
std::shared_ptr<CommandHandler> 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);
}
}
void GeneralCommandHandler::Handler(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_port,
uint8_t *buf, uint32_t buf_size) {
if (buf == nullptr || buf_size == 0) {
return;
}
if (cmd_observer_cb_) {
cmd_observer_cb_(handle, reinterpret_cast<LivoxLidarCmdPacket*>(buf), cmd_observer_client_data_);
}
if (dev_type == kLivoxLidarTypePA && lidar_port == kPaLidarFaultPort) {
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(dev_type);
if (cmd_handler == nullptr) {
LOG_ERROR("GeneralCommandHandler::Handler get cmd handler failed");
return;
}
Command command;
command.packet.data = buf;
command.packet.data_len = buf_size;
cmd_handler->Handle(handle, lidar_port, command);
return;
}
CommPacket packet;
memset(&packet, 0, sizeof(packet));
if (!(comm_port_->ParseCommStream((uint8_t*)buf, buf_size, &packet))) {
LOG_INFO("Parse Command Stream failed.");
return;
}
if (lidar_port == kDetectionPort && packet.cmd_id == kCommandIDLidarSearch) {
if (packet.cmd_type == kCommandTypeCmd) {
return;
}
HandleDetectionData(handle, lidar_port, packet);
return;
}
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(dev_type);
if (cmd_handler == nullptr) {
return;
}
Command command;
if (packet.cmd_type == kCommandTypeAck) {
uint16_t seq = packet.seq_num;
std::lock_guard<std::mutex> lock(commands_mutex_);
if (commands_.find(seq) != commands_.end()) {
command = commands_[seq].first;
command.packet = packet;
commands_.erase(seq);
}
} else if (packet.cmd_type == kCommandTypeCmd) {
command.packet = packet;
command.handle = handle;
}
cmd_handler->Handle(handle, lidar_port, command);
}
bool GeneralCommandHandler::VerifyNetSegment(const DetectionData* detection_data) {
if (is_view_) {
if (detection_host_ip_.empty()) {
LOG_ERROR("Verify net segment faield, the host ip is empty.");
return false;
}
std::vector<uint8_t> host_ip_vec;
if (!BuildRequest::IpToU8(detection_host_ip_, ".", host_ip_vec)) {
return false;
}
if (host_ip_vec[0] == detection_data->lidar_ip[0] &&
host_ip_vec[1] == detection_data->lidar_ip[1] &&
host_ip_vec[2] == detection_data->lidar_ip[2]) {
LOG_INFO("Host ip:{}, lidar ip:{}.{}.{}.{}", detection_host_ip_.c_str(), detection_data->lidar_ip[0],
detection_data->lidar_ip[1], detection_data->lidar_ip[2], detection_data->lidar_ip[3]);
return true;
}
std::string lidar_ip = std::to_string(detection_data->lidar_ip[0]) + "." +
std::to_string(detection_data->lidar_ip[1]) + "." +
std::to_string(detection_data->lidar_ip[2]) + "." +
std::to_string(detection_data->lidar_ip[3]);
LOG_ERROR("The host address and lidar address are on different network segments, the host_ip:{}, lidar_ip:{}",
detection_host_ip_.c_str(), lidar_ip.c_str());
return false;
}
return true;
}
void GeneralCommandHandler::CreateCommandHandler(const uint8_t dev_type) {
if (!is_view_) {
std::lock_guard<std::mutex> lock(command_handle_mutex_);
if (dev_type == kLivoxLidarTypeIndustrialHAP) {
if (!(lidars_command_handler_[dev_type]->Init(custom_lidars_cfg_map_))) {
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
}
} else if (dev_type == kLivoxLidarTypeMid360) {
if (!(lidars_command_handler_[dev_type]->Init(custom_lidars_cfg_map_))) {
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
}
} else if (dev_type == kLivoxLidarTypePA) {
if (!(lidars_command_handler_[dev_type]->Init(custom_lidars_cfg_map_))) {
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
}
} else if (dev_type == kLivoxLidarTypeMid360s) {
if (!(lidars_command_handler_[dev_type]->Init(custom_lidars_cfg_map_))) {
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
}
}
return;
}
std::lock_guard<std::mutex> lock(command_handle_mutex_);
if (lidars_command_handler_.find(dev_type) == lidars_command_handler_.end()) {
if (dev_type == kLivoxLidarTypeIndustrialHAP) {
std::shared_ptr<HapCommandHandler> hap_command_handler_ptr(new HapCommandHandler(device_manager_));
lidars_command_handler_[dev_type] = hap_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 == kLivoxLidarTypeMid360) {
std::shared_ptr<Mid360CommandHandler> 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 == kLivoxLidarTypeMid360s) {
std::shared_ptr<Mid360sCommandHandler> mid360s_command_handler_ptr(new Mid360sCommandHandler(device_manager_));
lidars_command_handler_[dev_type] = mid360s_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);
}
}
}
}
void GeneralCommandHandler::HandleDetectionData(uint32_t handle, uint16_t lidar_port, const CommPacket& packet) {
if (packet.data == nullptr || packet.data_len == 0) {
return;
}
DetectionData* detection_data = (DetectionData*)(packet.data);
if (detection_data->ret_code != 0) {
LOG_ERROR("Detection lidar faield, the handle:{}, lidar_port:{}, ret_code:{}",
handle, lidar_port, detection_data->ret_code);
return;
}
LOG_INFO("Handle detection data, handle:{}, dev_type:{}, sn:{}, cmd_port:{}",
handle, detection_data->dev_type, detection_data->sn, detection_data->cmd_port);
LoggerManager::GetInstance().AddDevice(handle, detection_data);
DebugPointCloudManager::GetInstance().AddDevice(handle, detection_data);
if (!VerifyNetSegment(detection_data)) {
return;
}
CreateCommandHandler(detection_data->dev_type);
std::string lidar_ip = std::to_string(detection_data->lidar_ip[0]) + "." +
std::to_string(detection_data->lidar_ip[1]) + "." +
std::to_string(detection_data->lidar_ip[2]) + "." +
std::to_string(detection_data->lidar_ip[3]);
if (devices_.find(handle) != devices_.end()) {
DeviceInfo& device_info = devices_[handle];
if (!(device_info.is_update_cfg.load()) && (device_info.is_get_loader_mode.load()) && !(device_info.is_loader_mode.load())) {
if (!is_view_) {
UpdateLidarCfg(detection_data->dev_type, handle, detection_data->cmd_port);
}
}
if (strcmp(device_info.sn.c_str(), detection_data->sn) != 0) {
LOG_ERROR("Lidar ip conflic, the lidar ip:{}, the sn1:{}, the sn2:{}", lidar_ip.c_str(),
device_info.sn.c_str(), detection_data->sn);
}
if (device_manager_) {
device_manager_->HandleDetectionData(handle, detection_data, device_info.is_get_loader_mode.load(),
device_info.is_loader_mode.load());
}
return;
}
{
std::lock_guard<std::mutex> lock(dev_type_mutex_);
if (device_dev_type_.find(handle) != device_dev_type_.end()) {
if (device_dev_type_[handle] != detection_data->dev_type) {
LOG_ERROR("Lidar dev type conflic, the lidar ip:{}, the dev_type1:{}, the dev_type2:{}",
lidar_ip.c_str(), device_dev_type_[handle], detection_data->dev_type);
}
} else {
device_dev_type_[handle] = detection_data->dev_type;
}
}
DeviceInfo& device_info = devices_[handle];
device_info.sn = detection_data->sn;
device_info.lidar_ip = lidar_ip;
device_info.dev_type = detection_data->dev_type;
device_info.is_get_loader_mode.store(false);
device_info.is_update_cfg.store(false);
device_info.is_callback.store(false);
GetFirmwareType(handle, device_info);
}
void GeneralCommandHandler::GetFirmwareType(const uint32_t handle, DeviceInfo& device_info) {
if (!device_manager_->sdk_framework_cfg_ptr_->master_sdk) {
return;
}
if (device_info.is_get_loader_mode.load()) {
return;
}
QueryFwType(handle);
}
void GeneralCommandHandler::QueryFwTypeCallback(livox_status status, uint32_t handle,
LivoxLidarDiagInternalInfoResponse* response, void* client_data) {
static int8_t count = 0;
if (count > 10) {
LOG_ERROR("Query livox lidar failed, the retry time more than 10.");
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
self->UpdateFwType(handle, 1);
count = 0;
return;
}
if (client_data == nullptr) {
LOG_ERROR("Query livox lidar Fw type failed, client data is nullptr.");
count += 1;
return;
}
if (status != kLivoxLidarStatusSuccess) {
LOG_ERROR("Query livox lidar Fw type failed, the status:{}", status);
count += 1;
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
self->QueryFwType(handle);
return;
}
if (response == nullptr) {
LOG_ERROR("Query livox lidar Fw type failed, the response is nullptr.");
count += 1;
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
self->QueryFwType(handle);
return;
}
if (response->ret_code != 0) {
LOG_ERROR("Query livox lidar Fw type failed, the ret_code:{}", response->ret_code);
count += 1;
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
self->QueryFwType(handle);
return;
}
// if (response->param_num != 1) {
// LOG_ERROR("Query livox lidar Fw type failed, the key_num:{}", response->param_num);
// count += 1;
// GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
// self->QueryFwType(handle);
// return;
// }
uint16_t off = 0;
LivoxLidarKeyValueParam* kv = (LivoxLidarKeyValueParam*)&response->data[off];
if (kv->key != kKeyFwType) {
LOG_ERROR("Query Fw type filed, the key had fault, the key:{}", kv->key);
count += 1;
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
self->QueryFwType(handle);
return;
}
off += sizeof(uint16_t) * 2;
if (kv->length != sizeof(uint8_t)) {
LOG_ERROR("Query Fw type failed, the val lenth is error, the len:", kv->length);
count += 1;
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
self->QueryFwType(handle);
return;
}
uint8_t fw_type = 0;
memcpy(&fw_type, &(response->data[off]), kv->length);
LOG_INFO("Query Fw type succ, the fw_type:{}", fw_type);
count = 0;
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
self->UpdateFwType(handle, fw_type);
}
void GeneralCommandHandler::UpdateFwType(const uint32_t handle, const uint8_t fw_type) {
if (devices_.find(handle) != devices_.end()) {
DeviceInfo& device_info = devices_[handle];
device_info.is_get_loader_mode.store(true);
if (fw_type) {
device_info.is_loader_mode.store(false);
} else {
device_info.is_loader_mode.store(true);
}
}
}
livox_status GeneralCommandHandler::QueryFwType(const uint32_t handle) {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
std::set<ParamKeyName> key_sets;
key_sets.insert(kKeyFwType);
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<uint16_t>(key);
req_len += sizeof(uint16_t);
}
return SendCommand(handle, kCommandIDLidarGetInternalInfo, req_buff, req_len,
MakeCommandCallback<LivoxLidarDiagInternalInfoResponse>(GeneralCommandHandler::QueryFwTypeCallback, this));
}
void GeneralCommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) {
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(view_lidar_info.dev_type);
if (cmd_handler != nullptr && device_manager_->sdk_framework_cfg_ptr_->master_sdk) {
cmd_handler->UpdateLidarCfg(view_lidar_info);
}
}
void GeneralCommandHandler::UpdateLidarCfg(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_cmd_port) {
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(dev_type);
if (cmd_handler != nullptr && device_manager_->sdk_framework_cfg_ptr_->master_sdk) {
cmd_handler->UpdateLidarCfg(handle, lidar_cmd_port);
}
}
void GeneralCommandHandler::LivoxLidarInfoChange(const uint32_t handle) {
LivoxLidarInfo lidar_info;
bool is_loader_mode = false;
bool is_callback = false;
{
devices_[handle].is_update_cfg.store(true);
if (devices_.find(handle) == devices_.end()) {
LOG_ERROR("Lidar info change failed, can not found device, the handle:{}", handle);
return;
}
DeviceInfo& device_info = devices_[handle];
is_loader_mode = device_info.is_loader_mode.load();
is_callback = device_info.is_callback.load();
device_info.is_callback.store(true);
strcpy(lidar_info.sn, device_info.sn.c_str());
strcpy(lidar_info.lidar_ip, device_info.lidar_ip.c_str());
lidar_info.dev_type = device_info.dev_type;
}
if (device_manager_ && is_view_ && !is_loader_mode) {
device_manager_->UpdateViewLidarCfgCallback(handle);
}
if (!is_callback) {
if (livox_lidar_info_change_cb_) {
livox_lidar_info_change_cb_(handle, &lidar_info, livox_lidar_info_change_client_data_);
}
}
}
void GeneralCommandHandler::PushLivoxLidarInfo(const uint32_t handle, const std::string& info) {
std::lock_guard<std::mutex> 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_);
}
}
}
std::shared_ptr<CommandHandler> GeneralCommandHandler::GetLidarCommandHandler(const uint32_t handle) {
std::lock_guard<std::mutex> 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);
}
LOG_ERROR("Get command handler failed, get dev type failed, the handle:{}", handle);
return nullptr;
}
std::shared_ptr<CommandHandler> GeneralCommandHandler::GetLidarCommandHandler(const uint8_t dev_type) {
std::lock_guard<std::mutex> lock(command_handle_mutex_);
if (lidars_command_handler_.find(dev_type) != lidars_command_handler_.end()) {
return lidars_command_handler_[dev_type];
}
return nullptr;
}
bool GeneralCommandHandler::GetQueryLidarInternalInfoKeys(const uint32_t handle, std::set<ParamKeyName>& key_sets) {
std::lock_guard<std::mutex> 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<ParamKeyName> tmp_key_sets {
kKeyPclDataType,
kKeyPatternMode,
kKeyDualEmitEn,
kKeyPointSendEn,
kKeyLidarIpCfg,
kKeyLidarPointDataHostIpCfg,
kKeyLidarImuHostIpCfg,
kKeyLogHostIpCfg,
kKeyInstallAttitude,
kKeyBlindSpotSet,
kKeyWorkMode,
kKeyGlassHeat,
kKeyImuDataEn,
kKeyFusaEn,
kKeyForceHeatEn,
kKeySn,
kKeyProductInfo,
kKeyVersionApp,
kKeyVersionLoader,
kKeyVersionHardware,
kKeyMac,
kKeyCurWorkState,
kKeyStatusCode,
kKeyLidarDiagStatus,
kKeyLidarFlashStatus,
kKeyFwType,
kKeyCurGlassHeatState
};
key_sets.swap(tmp_key_sets);
return true;
} else if (dev_type == kLivoxLidarTypeMid360) {
std::set<ParamKeyName> tmp_key_sets {
kKeyPclDataType,
kKeyPatternMode,
kKeyLidarIpCfg,
kKeyStateInfoHostIpCfg,
kKeyLidarPointDataHostIpCfg,
kKeyLidarImuHostIpCfg,
kKeyInstallAttitude,
kKeyFovCfg0,
kKeyFovCfg1,
kKeyFovCfgEn,
kKeyDetectMode,
kKeyFuncIoCfg,
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 == kLivoxLidarTypeMid360s){
std::set<ParamKeyName> tmp_key_sets {
kKeyPclDataType,
kKeyPatternMode,
kKeyLidarIpCfg,
kKeyStateInfoHostIpCfg,
kKeyLidarPointDataHostIpCfg,
kKeyLidarImuHostIpCfg,
kKeyInstallAttitude,
kKeyFovCfg0,
kKeyFovCfg1,
kKeyFovCfgEn,
kKeyDetectMode,
kKeyFuncIoCfg,
kKeyWorkMode,
kKeyImuDataEn,
kKeySetEscMode,
kKeySn,
kKeyProductInfo,
kKeyVersionApp,
kKeyVersionLoader,
kKeyVersionHardware,
kKeyMac,
kKeyCurWorkState,
kKeyCoreTemp,
kKeyPowerUpCnt,
kKeyLocalTimeNow,
kKeyLastSyncTime,
kKeyTimeOffset,
kKeyTimeSyncType,
kKeyLidarDiagStatus,
kKeyFwType,
kKeyHmsCode
};
key_sets.swap(tmp_key_sets);
return true;
}
}
return false;
}
const LivoxLidarCfg& GeneralCommandHandler::GetLidarCfg(const uint32_t handle) {
return custom_lidars_cfg_map_[handle];
}
livox_status GeneralCommandHandler::LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) {
LivoxLidarResetRequest reset_request;
std::string sn;
if (devices_.find(handle) != devices_.end()) {
sn = devices_[handle].sn;
} else {
return kLivoxLidarStatusChannelNotExist;
}
if (sn.size() > 16) {
LOG_ERROR("Request reset failed, the sn size too long, the sn:", sn.c_str());
return kLivoxLidarStatusChannelNotExist;
}
memcpy(reset_request.data, sn.c_str(), sn.size());
return SendCommand(handle, kCommandIDLidarResetDevice, (uint8_t*)&reset_request, sizeof(LivoxLidarResetRequest),
MakeCommandCallback<LivoxLidarResetResponse>(cb, client_data));
}
livox_status GeneralCommandHandler::SendCommand(uint32_t handle,
uint16_t command_id,
uint8_t *data,
uint16_t length,
const std::shared_ptr<CommandCallback> &cb) {
struct in_addr addr;
addr.s_addr = handle;
std::string lidar_ip = inet_ntoa(addr);
uint16_t seq = GenerateSeq::GetSeq();
Command command(seq, command_id, kCommandTypeCmd, kHostSend, data, length, handle, lidar_ip, cb);
std::shared_ptr<CommandHandler> 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);
AddCommand(command);
return kLivoxLidarStatusSuccess;
}
livox_status GeneralCommandHandler::SendLoggerCommand(uint32_t handle,
uint16_t command_id,
uint8_t *data,
uint16_t length,
const std::shared_ptr<CommandCallback> &cb) {
struct in_addr addr;
addr.s_addr = handle;
std::string lidar_ip = inet_ntoa(addr);
uint16_t seq = GenerateSeq::GetSeq();
Command command(seq, command_id, kCommandTypeCmd, kHostSend, data, length, handle, lidar_ip, cb);
std::shared_ptr<CommandHandler> 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->SendLoggerCommand(command);
AddCommand(command);
return kLivoxLidarStatusSuccess;
}
void GeneralCommandHandler::AddCommand(const Command& command) {
if (command.packet.cmd_type == kCommandTypeAck) {
return;
}
std::lock_guard<std::mutex> 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;
if (cmd.packet.data != NULL) {
cmd.packet.data = NULL;
cmd.packet.data_len = 0;
}
}
void GeneralCommandHandler::CommandsHandle(TimePoint now) {
std::list<Command> timeout_commands;
{
std::lock_guard<std::mutex> lock(commands_mutex_);
std::map<uint32_t, std::pair<Command, TimePoint> >::iterator ite = commands_.begin();
while (ite != commands_.end()) {
std::pair<Command, TimePoint> &command_pair = ite->second;
if (now > command_pair.second) {
timeout_commands.push_back(command_pair.first);
uint32_t seq = ite->first;
++ite;
commands_.erase(seq);
} else {
++ite;
}
}
}
for (auto& timeout_command : timeout_commands) {
if (timeout_command.cb) {
(*timeout_command.cb)(kLivoxLidarStatusTimeout, timeout_command.handle, timeout_command.packet.data);
}
}
}
} // namespace livox
} // namespace lidar
sdk_core/command_handler/general_command_handler.h
+163
View File
@@ -0,0 +1,163 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef GENERAL_COMMAND_HANDLER_H_
#define GENERAL_COMMAND_HANDLER_H_
#include <memory>
#include <map>
#include <condition_variable>
#include <mutex>
#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 {
typedef struct {
std::string sn;
std::string lidar_ip;
uint8_t dev_type;
std::atomic<bool> is_update_cfg;
std::atomic<bool> is_get_loader_mode;
std::atomic<bool> is_loader_mode;
std::atomic<bool> is_callback;
} DeviceInfo;
class HapCommandHandle;
class GeneralCommandHandler : public noncopyable {
private:
GeneralCommandHandler();
GeneralCommandHandler(const GeneralCommandHandler& other) = delete;
GeneralCommandHandler& operator=(const GeneralCommandHandler& other) = delete;
public:
~GeneralCommandHandler();
void Destory();
static GeneralCommandHandler& GetInstance();
bool Init(const std::string& host_ip, const bool is_view, DeviceManager* device_manager);
bool Init(std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr, DeviceManager* device_manager);
// void SetDeviceManager(DeviceManager* device_manager);
void Handler(uint32_t handle, uint16_t lidar_port, uint8_t *buf, uint32_t buf_size);
void Handler(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_port,
uint8_t *buf, uint32_t buf_size);
void CreateCommandHandler(const uint8_t dev_type);
livox_status SendCommand(uint32_t handle, uint16_t command_id, uint8_t *data,
uint16_t length, const std::shared_ptr<CommandCallback> &cb);
livox_status SendLoggerCommand(uint32_t handle, uint16_t command_id, uint8_t *data,
uint16_t length, const std::shared_ptr<CommandCallback> &cb);
void CommandsHandle(TimePoint now);
void AddCommand(const Command& command);
void AddDetectedLidar(const std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr);
void SetLivoxLidarInfoChangeCallback(LivoxLidarInfoChangeCallback cb, void* client_data) {
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 LivoxLidarAddCmdObserver(LivoxLidarCmdObserverCallBack cb, void* client_data) {
cmd_observer_cb_ = cb;
cmd_observer_client_data_ = client_data;
}
void LivoxLidarRemoveCmdObserver() {
cmd_observer_cb_ = nullptr;
cmd_observer_client_data_ = nullptr;
}
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<ParamKeyName>& key_sets);
const LivoxLidarCfg& GetLidarCfg(const uint32_t handle);
livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data);
static void QueryFwTypeCallback(livox_status status, uint32_t handle, LivoxLidarDiagInternalInfoResponse* response, void* client_data);
private:
bool VerifyNetSegment(const DetectionData* detection_data);
std::shared_ptr<CommandHandler> GetLidarCommandHandler(const uint8_t dev_type);
std::shared_ptr<CommandHandler> GetLidarCommandHandler(const uint32_t handle);
void HandleDetectionData(uint32_t handle, uint16_t lidar_port, const CommPacket& packet);
void GetFirmwareType(const uint32_t handle, DeviceInfo& device_info);
livox_status QueryFwType(const uint32_t handle);
void UpdateFwType(const uint32_t handle, const uint8_t fw_type);
private:
DeviceManager* device_manager_;
std::unique_ptr<CommPort> comm_port_;
std::map<uint32_t, LivoxLidarCfg> custom_lidars_cfg_map_;
std::mutex dev_type_mutex_;
std::map<uint32_t, uint8_t> device_dev_type_;
std::mutex devices_mutex_;
std::map<uint32_t, DeviceInfo> devices_;
std::mutex command_handle_mutex_;
std::map<uint8_t, std::shared_ptr<CommandHandler>> lidars_command_handler_;
std::mutex commands_mutex_;
std::map<uint32_t, std::pair<Command, TimePoint> > commands_;
LivoxLidarInfoChangeCallback livox_lidar_info_change_cb_;
void* livox_lidar_info_change_client_data_;
LivoxLidarInfoCallback livox_lidar_info_cb_;
void* livox_lidar_info_client_data_;
LivoxLidarCmdObserverCallBack cmd_observer_cb_{nullptr};
void* cmd_observer_client_data_{nullptr};
std::string detection_host_ip_;
bool is_view_;
};
} // namespace livox
} // namespace lidar
#endif // GENERAL_COMMAND_HANDLER_H_
sdk_core/command_handler/hap_command_handler.cpp
+292
View File
@@ -0,0 +1,292 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "hap_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 {
HapCommandHandler::HapCommandHandler(DeviceManager* device_manager)
: CommandHandler(device_manager),
comm_port_(new CommPort),
is_view_(false) {
}
bool HapCommandHandler::Init(bool is_view) {
is_view_ = is_view;
return true;
}
bool HapCommandHandler::Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map) {
for (const auto& it : custom_lidars_cfg_map) {
if (it.second.device_type == kLivoxLidarTypeIndustrialHAP && custom_lidars_.find(it.first) == custom_lidars_.end()) {
custom_lidars_[it.first] = it.second;
}
}
return true;
}
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);
} else if (command.packet.cmd_type == kCommandTypeCmd) {
LOG_INFO(" Receive Command: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
OnCommandCmd(handle, lidar_port, command);
}
}
void HapCommandHandler::OnCommandAck(uint32_t handle, const Command &command) {
if (command.cb == nullptr) {
return;
}
if (command.packet.data == nullptr) {
(*command.cb)(kLivoxLidarStatusTimeout, handle, command.packet.data);
return;
}
(*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data);
}
void HapCommandHandler::OnCommandCmd(const uint32_t handle, uint16_t lidar_port, const Command& command) {
if (command.packet.cmd_id == kCommandIDLidarPushMsg && lidar_port == kHAPPushMsgPort) {
std::string info;
ParseLidarStateInfo::Parse(command.packet, info);
GeneralCommandHandler::GetInstance().PushLivoxLidarInfo(handle, info);
}
}
void HapCommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) {
{
std::lock_guard<std::mutex> lock(device_mutex_);
if (devices_.find(view_lidar_info.handle) != devices_.end()) {
return;
}
}
SetViewLidar(view_lidar_info);
}
void HapCommandHandler::UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) {
{
std::lock_guard<std::mutex> lock(device_mutex_);
if (devices_.find(handle) != devices_.end()) {
return;
}
}
if (custom_lidars_.find(handle) != custom_lidars_.end()) {
const LivoxLidarCfg& lidar_cfg = custom_lidars_[handle];
SetCustomLidar(handle, lidar_cmd_port, lidar_cfg);
return;
}
}
void HapCommandHandler::SetViewLidar(const ViewLidarIpInfo& view_lidar_info) {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
if (!BuildRequest::BuildUpdateViewLidarCfgRequest(view_lidar_info, req_buff, req_len)) {
LOG_ERROR("Build update view lidar cfg request failed.");
return;
}
struct in_addr addr;
addr.s_addr = view_lidar_info.handle;
std::string lidar_ip = inet_ntoa(addr);
uint16_t seq = GenerateSeq::GetSeq();
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, view_lidar_info.handle,
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(HapCommandHandler::UpdateLidarCallback, this));
SendCommand(command, view_lidar_info.lidar_cmd_port);
}
void HapCommandHandler::SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg) {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
if (!BuildRequest::BuildUpdateLidarCfgRequest(lidar_cfg, req_buff, req_len)) {
LOG_ERROR("Build update lidar cfg request failed.");
return;
}
struct in_addr addr;
addr.s_addr = handle;
std::string lidar_ip = inet_ntoa(addr);
uint16_t seq = GenerateSeq::GetSeq();
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, handle,
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(HapCommandHandler::UpdateLidarCallback, this));
SendCommand(command, lidar_cmd_port);
}
void HapCommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response, void *client_data) {
if (status != kLivoxLidarStatusSuccess) {
LOG_INFO("Update lidar failed, the status:{}", status);
return;
}
if (response == nullptr) {
LOG_ERROR("Update lidar failed, the handle:{}, status:{}, response is nullptr.", handle, status);
return;
}
if (response->ret_code == 0 && response->error_key == 0) {
if (client_data != nullptr) {
HapCommandHandler* self = (HapCommandHandler*)client_data;
self->AddDevice(handle);
}
LOG_INFO("Update lidar:{} succ.", handle);
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
} else {
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
LOG_ERROR("Update lidar failed, the ret_code:{}, error_key:{}", response->ret_code, response->error_key);
}
}
void HapCommandHandler::AddDevice(const uint32_t handle) {
std::lock_guard<std::mutex> lock(device_mutex_);
devices_.insert(handle);
}
bool HapCommandHandler::IsStatusException(const Command &command) {
if (!command.packet.data) {
return false;
}
if (command.packet.cmd_id != kCommandIDLidarWorkModeControl) {
return false;
}
LivoxLidarAsyncControlResponse* data = (LivoxLidarAsyncControlResponse*)(command.packet.data);
if (data->ret_code != 0) {
return false;
}
return true;
}
livox_status HapCommandHandler::SendCommand(const Command &command, const uint16_t lidar_cmd_port) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
GeneralCommandHandler::GetInstance().AddCommand(command);
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(lidar_cmd_port);
//LOG_INFO("HapCommandHandler::SendCommand seq:{}, lidar_ip:{}", command.packet.seq_num, command.lidar_ip.c_str());
int byte_send = device_manager_->SendCommand(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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
livox_status HapCommandHandler::SendCommand(const Command &command) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
GeneralCommandHandler::GetInstance().AddCommand(command);
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(kHAPCmdPort);
//LOG_INFO("HapCommandHandler::SendCommand seq:{}, lidar_ip:{}", command.packet.seq_num, command.lidar_ip.c_str());
int byte_send = device_manager_->SendCommand(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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
livox_status HapCommandHandler::SendLoggerCommand(const Command &command) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
GeneralCommandHandler::GetInstance().AddCommand(command);
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(kHAPLogPort);
//LOG_INFO("HapCommandHandler::SendCommand seq:{}, lidar_ip:{}", command.packet.seq_num, command.lidar_ip.c_str());
int byte_send = device_manager_->SendLoggerCommand(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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
bool HapCommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) {
return true;
}
} // namespace livox
} // namespace lidar
sdk_core/command_handler/hap_command_handler.h
+91
View File
@@ -0,0 +1,91 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef HAP_COMMAND_HANDLER_H_
#define HAP_COMMAND_HANDLER_H_
#include <memory>
#include <map>
#include <list>
#include <mutex>
#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 HapCommandHandler : public CommandHandler {
public:
HapCommandHandler(DeviceManager* device_manager);
~HapCommandHandler() {}
static HapCommandHandler& GetInstance();
virtual bool Init(bool is_view);
virtual bool Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map);
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);
virtual livox_status SendLoggerCommand(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 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(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<CommPort> comm_port_;
std::mutex device_mutex_;
std::set<uint32_t> devices_;
std::map<uint32_t, LivoxLidarCfg> custom_lidars_;
bool is_view_;
};
} // namespace livox
} // namespace lidar
#endif // HAP_COMMAND_HANDLER_H_
sdk_core/command_handler/mid360_command_handler.cpp
+287
View File
@@ -0,0 +1,287 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "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),
is_view_(false) {
}
bool Mid360CommandHandler::Init(bool is_view) {
is_view_ = is_view;
return true;
}
bool Mid360CommandHandler::Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map) {
for (const auto& it : custom_lidars_cfg_map) {
if (it.second.device_type == kLivoxLidarTypeMid360 && custom_lidars_.find(it.first) == custom_lidars_.end()) {
custom_lidars_[it.first] = it.second;
}
}
return true;
}
void Mid360CommandHandler::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);
} else if (command.packet.cmd_type == kCommandTypeCmd) {
LOG_INFO(" Receive Command: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
OnCommandCmd(handle, lidar_port, command);
}
}
void Mid360CommandHandler::OnCommandAck(uint32_t handle, const Command &command) {
if (command.cb == nullptr) {
return;
}
if (command.packet.data == nullptr) {
(*command.cb)(kLivoxLidarStatusTimeout, handle, command.packet.data);
return;
}
(*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data);
}
void Mid360CommandHandler::OnCommandCmd(uint32_t handle, const uint16_t lidar_port, const Command &command) {
if (command.packet.cmd_id == kCommandIDLidarPushMsg && lidar_port == kMid360LidarPushMsgPort) {
std::string info;
ParseLidarStateInfo::Parse(command.packet, info);
GeneralCommandHandler::GetInstance().PushLivoxLidarInfo(handle, info);
}
}
void Mid360CommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) {
{
std::lock_guard<std::mutex> lock(device_mutex_);
if (devices_.find(view_lidar_info.handle) != devices_.end()) {
return;
}
}
SetViewLidar(view_lidar_info);
}
void Mid360CommandHandler::UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) {
{
std::lock_guard<std::mutex> lock(device_mutex_);
if (devices_.find(handle) != devices_.end()) {
return;
}
}
if (custom_lidars_.find(handle) != custom_lidars_.end()) {
const LivoxLidarCfg& lidar_cfg = custom_lidars_[handle];
SetCustomLidar(handle, lidar_cmd_port, lidar_cfg);
return;
}
}
void Mid360CommandHandler::SetViewLidar(const ViewLidarIpInfo& view_lidar_info) {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
if (!BuildRequest::BuildUpdateViewLidarCfgRequest(view_lidar_info, req_buff, req_len)) {
LOG_ERROR("Build update view lidar cfg request failed.");
return;
}
struct in_addr addr;
addr.s_addr = view_lidar_info.handle;
std::string lidar_ip = inet_ntoa(addr);
uint16_t seq = GenerateSeq::GetSeq();
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, view_lidar_info.handle,
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(Mid360CommandHandler::UpdateLidarCallback, this));
SendCommand(command, view_lidar_info.lidar_cmd_port);
}
void Mid360CommandHandler::SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg) {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
if (!BuildRequest::BuildUpdateMid360LidarCfgRequest(lidar_cfg, req_buff, req_len)) {
LOG_ERROR("Build update lidar cfg request failed.");
return;
}
struct in_addr addr;
addr.s_addr = handle;
std::string lidar_ip = inet_ntoa(addr);
uint16_t seq = GenerateSeq::GetSeq();
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, handle,
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(Mid360CommandHandler::UpdateLidarCallback, this));
SendCommand(command, lidar_cmd_port);
}
void Mid360CommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response, void *client_data) {
if (status != kLivoxLidarStatusSuccess) {
LOG_INFO("Update lidar failed, the status:{}", status);
return;
}
if (response == nullptr) {
LOG_ERROR("Update lidar failed, the handle:{}, status:{}, response is nullptr.", handle, status);
return;
}
if (response->ret_code == 0 && response->error_key == 0) {
if (client_data != nullptr) {
Mid360CommandHandler* self = (Mid360CommandHandler*)client_data;
self->AddDevice(handle);
}
LOG_INFO("Update lidar:{} succ.", handle);
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
} else {
//GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
LOG_ERROR("Update lidar failed, the ret_code:{}, error_key:{}", response->ret_code, response->error_key);
}
}
void Mid360CommandHandler::AddDevice(const uint32_t handle) {
std::lock_guard<std::mutex> lock(device_mutex_);
devices_.insert(handle);
}
bool Mid360CommandHandler::IsStatusException(const Command &command) {
if (!command.packet.data) {
return false;
}
if (command.packet.cmd_id != kCommandIDLidarWorkModeControl) {
return false;
}
LivoxLidarAsyncControlResponse* data = (LivoxLidarAsyncControlResponse*)(command.packet.data);
if (data->ret_code != 0) {
return false;
}
return true;
}
livox_status Mid360CommandHandler::SendCommand(const Command &command, const uint16_t lidar_cmd_port) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
GeneralCommandHandler::GetInstance().AddCommand(command);
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(lidar_cmd_port);
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeMid360, 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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
livox_status Mid360CommandHandler::SendCommand(const Command &command) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(kMid360LidarCmdPort);
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeMid360, 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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
livox_status Mid360CommandHandler::SendLoggerCommand(const Command &command) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(kMid360LidarLogPort);
int byte_send = device_manager_->SendLoggerCommand(kLivoxLidarTypeMid360, 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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
bool Mid360CommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) {
return true;
}
} // namespace livox
} // namespace lidar
sdk_core/command_handler/mid360_command_handler.h
+90
View File
@@ -0,0 +1,90 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef MID360_COMMAND_HANDLER_H_
#define MID360_COMMAND_HANDLER_H_
#include <memory>
#include <map>
#include <list>
#include <mutex>
#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(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map);
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);
virtual livox_status SendLoggerCommand(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 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<CommPort> comm_port_;
std::mutex device_mutex_;
std::set<uint32_t> devices_;
std::map<uint32_t, LivoxLidarCfg> custom_lidars_;
bool is_view_;
};
} // namespace livox
} // namespace lidar
#endif // MID360_COMMAND_HANDLER_H_
sdk_core/command_handler/mid360s_command_handler.cpp
+287
View File
@@ -0,0 +1,287 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "mid360s_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 {
Mid360sCommandHandler::Mid360sCommandHandler(DeviceManager* device_manager)
: CommandHandler(device_manager),
comm_port_(new CommPort),
is_view_(false) {
}
bool Mid360sCommandHandler::Init(bool is_view) {
is_view_ = is_view;
return true;
}
bool Mid360sCommandHandler::Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map) {
for (const auto& it : custom_lidars_cfg_map) {
if (it.second.device_type == kLivoxLidarTypeMid360s && custom_lidars_.find(it.first) == custom_lidars_.end()) {
custom_lidars_[it.first] = it.second;
}
}
return true;
}
void Mid360sCommandHandler::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);
} else if (command.packet.cmd_type == kCommandTypeCmd) {
LOG_INFO(" Receive Command: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
OnCommandCmd(handle, lidar_port, command);
}
}
void Mid360sCommandHandler::OnCommandAck(uint32_t handle, const Command &command) {
if (command.cb == nullptr) {
return;
}
if (command.packet.data == nullptr) {
(*command.cb)(kLivoxLidarStatusTimeout, handle, command.packet.data);
return;
}
(*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data);
}
void Mid360sCommandHandler::OnCommandCmd(uint32_t handle, const uint16_t lidar_port, const Command &command) {
if (command.packet.cmd_id == kCommandIDLidarPushMsg && lidar_port == kMid360sLidarPushMsgPort) {
std::string info;
ParseLidarStateInfo::Parse(command.packet, info);
GeneralCommandHandler::GetInstance().PushLivoxLidarInfo(handle, info);
}
}
void Mid360sCommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) {
{
std::lock_guard<std::mutex> lock(device_mutex_);
if (devices_.find(view_lidar_info.handle) != devices_.end()) {
return;
}
}
SetViewLidar(view_lidar_info);
}
void Mid360sCommandHandler::UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) {
{
std::lock_guard<std::mutex> lock(device_mutex_);
if (devices_.find(handle) != devices_.end()) {
return;
}
}
if (custom_lidars_.find(handle) != custom_lidars_.end()) {
const LivoxLidarCfg& lidar_cfg = custom_lidars_[handle];
SetCustomLidar(handle, lidar_cmd_port, lidar_cfg);
return;
}
}
void Mid360sCommandHandler::SetViewLidar(const ViewLidarIpInfo& view_lidar_info) {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
if (!BuildRequest::BuildUpdateViewLidarCfgRequest(view_lidar_info, req_buff, req_len)) {
LOG_ERROR("Build update view lidar cfg request failed.");
return;
}
struct in_addr addr;
addr.s_addr = view_lidar_info.handle;
std::string lidar_ip = inet_ntoa(addr);
uint16_t seq = GenerateSeq::GetSeq();
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, view_lidar_info.handle,
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(Mid360sCommandHandler::UpdateLidarCallback, this));
SendCommand(command, view_lidar_info.lidar_cmd_port);
}
void Mid360sCommandHandler::SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg) {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
if (!BuildRequest::BuildUpdateMid360LidarCfgRequest(lidar_cfg, req_buff, req_len)) {
LOG_ERROR("Build update lidar cfg request failed.");
return;
}
struct in_addr addr;
addr.s_addr = handle;
std::string lidar_ip = inet_ntoa(addr);
uint16_t seq = GenerateSeq::GetSeq();
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, handle,
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(Mid360sCommandHandler::UpdateLidarCallback, this));
SendCommand(command, lidar_cmd_port);
}
void Mid360sCommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle,
LivoxLidarAsyncControlResponse *response, void *client_data) {
if (status != kLivoxLidarStatusSuccess) {
LOG_INFO("Update lidar failed, the status:{}", status);
return;
}
if (response == nullptr) {
LOG_ERROR("Update lidar failed, the handle:{}, status:{}, response is nullptr.", handle, status);
return;
}
if (response->ret_code == 0 && response->error_key == 0) {
if (client_data != nullptr) {
Mid360sCommandHandler* self = (Mid360sCommandHandler*)client_data;
self->AddDevice(handle);
}
LOG_INFO("Update lidar:{} succ.", handle);
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
} else {
//GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
LOG_ERROR("Update lidar failed, the ret_code:{}, error_key:{}", response->ret_code, response->error_key);
}
}
void Mid360sCommandHandler::AddDevice(const uint32_t handle) {
std::lock_guard<std::mutex> lock(device_mutex_);
devices_.insert(handle);
}
bool Mid360sCommandHandler::IsStatusException(const Command &command) {
if (!command.packet.data) {
return false;
}
if (command.packet.cmd_id != kCommandIDLidarWorkModeControl) {
return false;
}
LivoxLidarAsyncControlResponse* data = (LivoxLidarAsyncControlResponse*)(command.packet.data);
if (data->ret_code != 0) {
return false;
}
return true;
}
livox_status Mid360sCommandHandler::SendCommand(const Command &command, const uint16_t lidar_cmd_port) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
GeneralCommandHandler::GetInstance().AddCommand(command);
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(lidar_cmd_port);
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeMid360s, 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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
livox_status Mid360sCommandHandler::SendCommand(const Command &command) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(kMid360sLidarCmdPort);
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeMid360s, 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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
livox_status Mid360sCommandHandler::SendLoggerCommand(const Command &command) {
if (command.packet.cmd_type == kCommandTypeAck) {
return kLivoxLidarStatusFailure;
}
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
servaddr.sin_port = htons(kMid360sLidarLogPort);
int byte_send = device_manager_->SendLoggerCommand(kLivoxLidarTypeMid360s, 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:{}",
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
if (command.cb) {
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
}
return kLivoxLidarStatusSendFailed;
}
return kLivoxLidarStatusSuccess;
}
bool Mid360sCommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) {
return true;
}
} // namespace livox
} // namespace lidar
sdk_core/command_handler/mid360s_command_handler.h
+90
View File
@@ -0,0 +1,90 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef MID360S_COMMAND_HANDLER_H_
#define MID360S_COMMAND_HANDLER_H_
#include <memory>
#include <map>
#include <list>
#include <mutex>
#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 Mid360sCommandHandler : public CommandHandler {
public:
Mid360sCommandHandler(DeviceManager* device_manager);
~Mid360sCommandHandler() {}
virtual bool Init(bool is_view);
virtual bool Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map);
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);
virtual livox_status SendLoggerCommand(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 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<CommPort> comm_port_;
std::mutex device_mutex_;
std::set<uint32_t> devices_;
std::map<uint32_t, LivoxLidarCfg> custom_lidars_;
bool is_view_;
};
} // namespace livox
} // namespace lidar
#endif // MID360S_COMMAND_HANDLER_H_
sdk_core/command_handler/parse_lidar_state_info.cpp
+725
View File
@@ -0,0 +1,725 @@
#include "parse_lidar_state_info.h"
#include "base/logging.h"
#include <iostream>
#include <sstream>
namespace livox {
namespace lidar {
bool ParseLidarStateInfo::Parse(const CommPacket& packet, std::string& info_str) {
DirectLidarStateInfo info;
std::set<ParamKeyName> key_mask;
if (!ParseStateInfo(packet, info, key_mask)) {
return false;
}
LivoxLidarStateInfoToJson(info, key_mask, info_str);
return true;
}
bool ParseLidarStateInfo::ParseStateInfo(const CommPacket& packet,
DirectLidarStateInfo& info,
std::set<ParamKeyName>& key_mask) {
uint16_t offset = 0;
uint16_t key_num = 0;
memcpy(&key_num, &packet.data[offset], sizeof(uint16_t));
offset += sizeof(uint16_t) * 2;
for (uint16_t i = 0; i < key_num; ++i) {
if (offset + sizeof(LivoxLidarKeyValueParam) > packet.data_len) {
return false;
}
LivoxLidarKeyValueParam* kv = (LivoxLidarKeyValueParam*)&packet.data[offset];
offset += sizeof(uint16_t);
uint16_t val_len = 0;
memcpy(&val_len, &packet.data[offset], sizeof(uint16_t));
offset += sizeof(uint16_t);
switch (kv->key) {
case static_cast<uint16_t>(kKeyPclDataType) :
key_mask.insert(kKeyPclDataType);
memcpy(&info.pcl_data_type, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyPatternMode) :
key_mask.insert(kKeyPatternMode);
memcpy(&info.pattern_mode, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyDualEmitEn) :
key_mask.insert(kKeyDualEmitEn);
memcpy(&info.dual_emit_en, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyPointSendEn) :
key_mask.insert(kKeyPointSendEn);
memcpy(&info.point_send_en, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyLidarIpCfg) :
key_mask.insert(kKeyLidarIpCfg);
ParseLidarIpAddr(packet, offset, info);
break;
case static_cast<uint16_t>(kKeyStateInfoHostIpCfg) :
key_mask.insert(kKeyStateInfoHostIpCfg);
ParseStateInfoHostIPCfg(packet, offset, info);
break;
case static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg) :
key_mask.insert(kKeyLidarPointDataHostIpCfg);
ParsePointCloudHostIpCfg(packet, offset, info);
break;
case static_cast<uint16_t>(kKeyLidarImuHostIpCfg) :
key_mask.insert(kKeyLidarImuHostIpCfg);
ParseImuDataHostIpCfg(packet, offset, info);
break;
case static_cast<uint16_t>(kKeyCtlHostIpCfg) :
key_mask.insert(kKeyCtlHostIpCfg);
ParseIpCfg(packet, offset, info.ctl_host_ipcfg);
break;
case static_cast<uint16_t>(kKeyLogHostIpCfg) :
key_mask.insert(kKeyLogHostIpCfg);
ParseIpCfg(packet, offset, info.log_host_ipcfg);
break;
case static_cast<uint16_t>(kKeyVehicleSpeed) :
key_mask.insert(kKeyVehicleSpeed);
memcpy(&info.vehicle_speed, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyEnvironmentTemp) :
key_mask.insert(kKeyEnvironmentTemp);
memcpy(&info.environment_temp, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyInstallAttitude) :
key_mask.insert(kKeyInstallAttitude);
memcpy(&info.install_attitude, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyBlindSpotSet) :
key_mask.insert(kKeyBlindSpotSet);
memcpy(&info.blind_spot_set, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyFrameRate) :
key_mask.insert(kKeyFrameRate);
memcpy(&info.frame_rate, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyFovCfg0) :
key_mask.insert(kKeyFovCfg0);
memcpy(&info.fov_cfg0, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyFovCfg1) :
key_mask.insert(kKeyFovCfg1);
memcpy(&info.fov_cfg1, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyFovCfgEn) :
key_mask.insert(kKeyFovCfgEn);
memcpy(&info.fov_cfg_en, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyDetectMode) :
key_mask.insert(kKeyDetectMode);
memcpy(&info.detect_mode, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyFuncIoCfg) :
key_mask.insert(kKeyFuncIoCfg);
memcpy(&info.func_io_cfg, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyWorkMode) :
key_mask.insert(kKeyWorkMode);
memcpy(&info.work_tgt_mode, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyGlassHeat) :
key_mask.insert(kKeyGlassHeat);
memcpy(&info.glass_heat, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyImuDataEn) :
key_mask.insert(kKeyImuDataEn);
memcpy(&info.imu_data_en, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyFusaEn) :
key_mask.insert(kKeyFusaEn);
memcpy(&info.fusa_en, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeySn) :
key_mask.insert(kKeySn);
memcpy(info.sn, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyProductInfo) :
key_mask.insert(kKeyProductInfo);
memcpy(info.product_info, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyVersionApp) :
key_mask.insert(kKeyVersionApp);
memcpy(info.version_app, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyVersionLoader) :
key_mask.insert(kKeyVersionLoader);
memcpy(info.version_loader, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyVersionHardware):
key_mask.insert(kKeyVersionHardware);
memcpy(info.version_hardware, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyMac) :
key_mask.insert(kKeyMac);
memcpy(info.mac, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyCurWorkState) :
key_mask.insert(kKeyCurWorkState);
memcpy(&info.cur_work_state, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyCoreTemp) :
key_mask.insert(kKeyCoreTemp);
memcpy(&info.core_temp, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyPowerUpCnt) :
key_mask.insert(kKeyPowerUpCnt);
memcpy(&info.powerup_cnt, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyLocalTimeNow) :
key_mask.insert(kKeyLocalTimeNow);
memcpy(&info.local_time_now, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyLastSyncTime) :
key_mask.insert(kKeyLastSyncTime);
memcpy(&info.last_sync_time, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyTimeOffset) :
key_mask.insert(kKeyTimeOffset);
memcpy(&info.time_offset, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyTimeSyncType) :
key_mask.insert(kKeyTimeSyncType);
memcpy(&info.time_sync_type, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyStatusCode) :
key_mask.insert(kKeyStatusCode);
memcpy(&info.status_code, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyLidarDiagStatus) :
key_mask.insert(kKeyLidarDiagStatus);
memcpy(&info.lidar_diag_status, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyLidarFlashStatus) :
key_mask.insert(kKeyLidarFlashStatus);
memcpy(&info.lidar_flash_status, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyFwType) :
key_mask.insert(kKeyFwType);
memcpy(&info.fw_type, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyHmsCode) :
key_mask.insert(kKeyHmsCode);
memcpy(&info.hms_code, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeyRoiMode) :
key_mask.insert(kKeyRoiMode);
memcpy(&info.ROI_Mode, &packet.data[offset], val_len);
break;
case static_cast<uint16_t>(kKeySetEscMode) :
key_mask.insert(kKeySetEscMode);
memcpy(&info.esc_mode, &packet.data[offset], val_len);
break;
default :
break;
}
offset += 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.lidar_ip_info.ip_addr, info.lidar_ip_info.net_mask,
// info.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.pointcloud_host_ipcfg.host_ip_addr, info.pointcloud_host_ipcfg.host_point_data_port,
// info.pointcloud_host_ipcfg.lidar_point_data_port);
// printf("Lidar state info, host_ip_addr:%s, host_imu_data_port:%u, lidar_imu_data_port:%u.\n",
// info.imu_host_ipcfg.host_ip_addr, info.pointcloud_host_ipcfg.host_point_data_port,
// info.imu_host_ipcfg.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, fov cfg0, yaw_start:%d, yaw_stop:%d, pitch_start:%d, pitch_stop:%d.\n",
// info.fov_cfg0.yaw_start, info.fov_cfg0.yaw_stop, info.fov_cfg0.pitch_start, info.fov_cfg0.pitch_stop);
// printf("Lidar state info, fov cfg1, yaw_start:%d, yaw_stop:%d, pitch_start:%d, pitch_stop:%d.\n",
// info.fov_cfg1.yaw_start, info.fov_cfg1.yaw_stop, info.fov_cfg1.pitch_start, info.fov_cfg1.pitch_stop);
// printf("Lidar state info, fov_en:%u, work_mode:%u, imu_data_en:%u, sn:%s, product_info:%s.\n",
// info.fov_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.lidar_ipcfg.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.lidar_ipcfg.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.lidar_ipcfg.gw_addr, lidar_gateway_str.c_str());
}
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));
}
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.pointcloud_host_ipcfg.host_ip_addr, host_point_cloud_ip_str.c_str());
off += sizeof(uint8_t) * 4;
memcpy(&info.pointcloud_host_ipcfg.host_point_data_port, &packet.data[off], sizeof(uint16_t));
off += sizeof(uint16_t);
memcpy(&info.pointcloud_host_ipcfg.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.imu_host_ipcfg.host_ip_addr, host_imu_data_ip_str.c_str());
off += sizeof(uint8_t) * 4;
memcpy(&info.imu_host_ipcfg.host_imu_data_port, &packet.data[off], sizeof(uint16_t));
off += sizeof(uint16_t);
memcpy(&info.imu_host_ipcfg.lidar_imu_data_port, &packet.data[off], sizeof(uint16_t));
off += sizeof(uint16_t);
}
void ParseLidarStateInfo::ParseIpCfg(const CommPacket& packet, uint16_t off, LivoxIpCfg& cfg) {
std::string ip_str = std::to_string(packet.data[off]) + "." +
std::to_string(packet.data[off + 1]) + "." +
std::to_string(packet.data[off + 2]) + "." +
std::to_string(packet.data[off + 3]);
strcpy(cfg.ip_addr, ip_str.c_str());
off += sizeof(uint8_t) * 4;
cfg.dst_port = *(uint16_t*)&packet.data[off];
off += sizeof(uint16_t);
cfg.src_port = *(uint16_t*)&packet.data[off];
return;
}
void ParseLidarStateInfo::LivoxLidarStateInfoToJson(const DirectLidarStateInfo& info, const std::set<ParamKeyName>& key_mask, std::string& lidar_info) {
rapidjson::StringBuffer buf;
rapidjson::PrettyWriter<rapidjson::StringBuffer> write(buf);
write.StartObject();
// write.Key("dev_type");
// write.String("MID360");
if (key_mask.find(kKeyPclDataType) != key_mask.end()) {
write.Key("pcl_data_type");
write.Uint(info.pcl_data_type);
}
if (key_mask.find(kKeyPatternMode) != key_mask.end()) {
write.Key("pattern_mode");
write.Uint(info.pattern_mode);
}
if (key_mask.find(kKeyDualEmitEn) != key_mask.end()) {
write.Key("dual_emit_en");
write.Uint(info.dual_emit_en);
}
if (key_mask.find(kKeyPointSendEn) != key_mask.end()) {
write.Key("point_send_en");
write.Uint(info.point_send_en);
}
if (key_mask.find(kKeyLidarIpCfg) != key_mask.end()) {
write.Key("lidar_ipcfg");
write.StartObject();
write.Key("lidar_ip");
write.String(info.lidar_ipcfg.ip_addr);
write.Key("lidar_subnet_mask");
write.String(info.lidar_ipcfg.net_mask);
write.Key("lidar_gateway");
write.String(info.lidar_ipcfg.gw_addr);
write.EndObject();
}
if (key_mask.find(kKeyStateInfoHostIpCfg) != key_mask.end()) {
write.Key("state_info_host_ipcfg");
write.StartObject();
write.Key("ip");
write.String(info.host_state_info.host_ip_addr);
write.Key("dst_port");
write.Uint(info.host_state_info.host_state_info_port);
write.Key("src_port");
write.Uint(info.host_state_info.lidar_state_info_port);
write.EndObject();
}
if (key_mask.find(kKeyLidarPointDataHostIpCfg) != key_mask.end()) {
write.Key("ponitcloud_host_ipcfg");
write.StartObject();
write.Key("ip");
write.String(info.pointcloud_host_ipcfg.host_ip_addr);
write.Key("dst_port");
write.Uint(info.pointcloud_host_ipcfg.host_point_data_port);
write.Key("src_port");
write.Uint(info.pointcloud_host_ipcfg.lidar_point_data_port);
write.EndObject();
}
if (key_mask.find(kKeyLidarImuHostIpCfg) != key_mask.end()) {
write.Key("imu_host_ipcfg");
write.StartObject();
write.Key("ip");
write.String(info.imu_host_ipcfg.host_ip_addr);
write.Key("dst_port");
write.Uint(info.imu_host_ipcfg.host_imu_data_port);
write.Key("src_port");
write.Uint(info.imu_host_ipcfg.lidar_imu_data_port);
write.EndObject();
}
if (key_mask.find(kKeyCtlHostIpCfg) != key_mask.end()) {
write.Key("ctl_host_ipcfg");
write.StartObject();
write.Key("ip");
write.String(info.ctl_host_ipcfg.ip_addr);
write.Key("dst_port");
write.Uint(info.ctl_host_ipcfg.dst_port);
write.Key("src_port");
write.Uint(info.ctl_host_ipcfg.src_port);
write.EndObject();
}
if (key_mask.find(kKeyLogHostIpCfg) != key_mask.end()) {
write.Key("log_host_ipcfg");
write.StartObject();
write.Key("ip");
write.String(info.log_host_ipcfg.ip_addr);
write.Key("dst_port");
write.Uint(info.log_host_ipcfg.dst_port);
write.Key("src_port");
write.Uint(info.log_host_ipcfg.src_port);
write.EndObject();
}
if (key_mask.find(kKeyVehicleSpeed) != key_mask.end()) {
write.Key("vehicle_speed");
write.Int(info.vehicle_speed);
}
if (key_mask.find(kKeyEnvironmentTemp) != key_mask.end()) {
write.Key("environment_temp");
write.Int(info.environment_temp);
}
if (key_mask.find(kKeyInstallAttitude) != key_mask.end()) {
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_mm");
write.Uint(info.install_attitude.x);
write.Key("y_mm");
write.Uint(info.install_attitude.y);
write.Key("z_mm");
write.Uint(info.install_attitude.z);
write.EndObject();
}
if (key_mask.find(kKeyBlindSpotSet) != key_mask.end()) {
write.Key("blind_spot_set");
write.Uint(info.blind_spot_set);
}
if (key_mask.find(kKeyFrameRate) != key_mask.end()) {
write.Key("frame_rate");
write.Uint(info.frame_rate);
}
if (key_mask.find(kKeyFovCfg0) != key_mask.end()) {
write.Key("fov_cfg0");
write.StartObject();
write.Key("yaw_start");
write.Int(info.fov_cfg0.yaw_start);
write.Key("yaw_stop");
write.Int(info.fov_cfg0.yaw_stop);
write.Key("pitch_start");
write.Int(info.fov_cfg0.pitch_start);
write.Key("pitch_stop");
write.Int(info.fov_cfg0.pitch_stop);
write.EndObject();
}
if (key_mask.find(kKeyFovCfg1) != key_mask.end()) {
write.Key("fov_cfg1");
write.StartObject();
write.Key("yaw_start");
write.Int(info.fov_cfg1.yaw_start);
write.Key("yaw_stop");
write.Int(info.fov_cfg1.yaw_stop);
write.Key("pitch_start");
write.Int(info.fov_cfg1.pitch_start);
write.Key("pitch_stop");
write.Int(info.fov_cfg1.pitch_stop);
write.EndObject();
}
if (key_mask.find(kKeyFovCfgEn) != key_mask.end()) {
write.Key("fov_cfg_en");
write.Uint(info.fov_cfg_en);
}
if (key_mask.find(kKeyDetectMode) != key_mask.end()) {
write.Key("detect_mode");
write.Uint(info.detect_mode);
}
if (key_mask.find(kKeyFuncIoCfg) != key_mask.end()) {
write.Key("func_io_cfg");
write.StartObject();
write.Key("IN0");
write.Uint(info.func_io_cfg[0]);
write.Key("IN1");
write.Uint(info.func_io_cfg[1]);
write.Key("OUT0");
write.Uint(info.func_io_cfg[2]);
write.Key("OUT1");
write.Uint(info.func_io_cfg[3]);
write.EndObject();
}
if (key_mask.find(kKeyWorkMode) != key_mask.end()) {
write.Key("work_tgt_mode");
write.Uint(info.work_tgt_mode);
}
if (key_mask.find(kKeyGlassHeat) != key_mask.end()) {
write.Key("glass_heat");
write.Uint(info.glass_heat);
}
if (key_mask.find(kKeyImuDataEn) != key_mask.end()) {
write.Key("imu_data_en");
write.Uint(info.imu_data_en);
}
if (key_mask.find(kKeyFusaEn) != key_mask.end()) {
write.Key("fusa_en");
write.Uint(info.fusa_en);
}
if (key_mask.find(kKeySetEscMode) != key_mask.end()) {
write.Key("esc_mode");
write.Uint(info.esc_mode);
}
if (key_mask.find(kKeySn) != key_mask.end()) {
write.Key("sn");
write.String(info.sn);
}
if (key_mask.find(kKeyProductInfo) != key_mask.end()) {
write.Key("product_info");
write.String(info.product_info);
}
if (key_mask.find(kKeyVersionApp) != key_mask.end()) {
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();
}
if (key_mask.find(kKeyVersionLoader) != key_mask.end()) {
write.Key("version_loader");
write.StartArray();
write.Uint(info.version_loader[0]);
write.Uint(info.version_loader[1]);
write.Uint(info.version_loader[2]);
write.Uint(info.version_loader[3]);
write.EndArray();
}
if (key_mask.find(kKeyVersionHardware) != key_mask.end()) {
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();
}
if (key_mask.find(kKeyMac) != key_mask.end()) {
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();
}
if (key_mask.find(kKeyCurWorkState) != key_mask.end()) {
write.Key("cur_work_state");
write.Uint(info.cur_work_state);
}
if (key_mask.find(kKeyCoreTemp) != key_mask.end()) {
write.Key("core_temp");
write.Int(info.core_temp);
}
if (key_mask.find(kKeyPowerUpCnt) != key_mask.end()) {
write.Key("powerup_cnt");
write.Uint(info.powerup_cnt);
}
if (key_mask.find(kKeyLocalTimeNow) != key_mask.end()) {
write.Key("local_time_now");
write.Uint64(info.local_time_now);
}
if (key_mask.find(kKeyLastSyncTime) != key_mask.end()) {
write.Key("last_sync_time");
write.Uint64(info.last_sync_time);
}
if (key_mask.find(kKeyTimeOffset) != key_mask.end()) {
write.Key("time_offset");
write.Int64(info.time_offset);
}
if (key_mask.find(kKeyTimeSyncType) != key_mask.end()) {
write.Key("time_sync_type");
write.Uint(info.time_sync_type);
}
if (key_mask.find(kKeyStatusCode) != key_mask.end()) {
write.Key("status_code");
std::ostringstream ss;
for (int idx = 31; idx >= 0; --idx) {
ss << std::hex << static_cast<uint32_t>(info.status_code[idx]);
if (idx != 0) {
ss << " ";
}
}
write.String(ss.str().c_str());
}
if (key_mask.find(kKeyLidarDiagStatus) != key_mask.end()) {
write.Key("lidar_diag_status");
write.Uint(info.lidar_diag_status);
}
if (key_mask.find(kKeyLidarFlashStatus) != key_mask.end()) {
write.Key("lidar_flash_status");
write.Uint(info.lidar_flash_status);
}
if (key_mask.find(kKeyFwType) != key_mask.end()) {
write.Key("FW_TYPE");
write.Uint(info.fw_type);
}
if (key_mask.find(kKeyHmsCode) != key_mask.end()) {
write.Key("hms_code");
write.StartArray();
write.Uint(info.hms_code[0]);
write.Uint(info.hms_code[1]);
write.Uint(info.hms_code[2]);
write.Uint(info.hms_code[3]);
write.Uint(info.hms_code[4]);
write.Uint(info.hms_code[5]);
write.Uint(info.hms_code[6]);
write.Uint(info.hms_code[7]);
write.EndArray();
}
if (key_mask.find(kKeyRoiMode) != key_mask.end()) {
write.Key("ROI_Mode");
write.Uint(info.ROI_Mode);
}
write.EndObject();
lidar_info = buf.GetString();
// LOG_INFO("###################################lidar_info_to_json:{}", lidar_info.c_str());
}
} // namespace livox
} // namespace direct
sdk_core/command_handler/parse_lidar_state_info.h
+83
View File
@@ -0,0 +1,83 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef PARSE_LIDAR_STATE_INFO_H_
#define PARSE_LIDAR_STATE_INFO_H_
#include <memory>
#include <map>
#include <mutex>
#include <set>
#include "comm/define.h"
#include "comm/protocol.h"
#include "livox_lidar_def.h"
#include "rapidjson/document.h"
#include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/prettywriter.h"
namespace livox {
namespace lidar {
class ParseLidarStateInfo {
public:
static bool Parse(const CommPacket& packet, std::string& info);
private:
static bool ParseStateInfo(const CommPacket& packet, DirectLidarStateInfo& info, std::set<ParamKeyName>& key_mask);
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 ParseIpCfg(const CommPacket& packet, uint16_t off, LivoxIpCfg& cfg);
static void LivoxLidarStateInfoToJson(const DirectLidarStateInfo& info, const std::set<ParamKeyName>& key_mask, std::string& lidar_info);
};
} // namespace livox
} // namespace lidar
# endif // PARSE_LIDAR_STATE_INFO_H_
sdk_core/data_handler/data_handler.cpp
+138
View File
@@ -0,0 +1,138 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "data_handler.h"
#include <base/logging.h>
#include "livox_lidar_def.h"
namespace livox {
namespace lidar {
static const size_t kPrefixDataSize = 18;
DataHandler::DataHandler()
: point_data_callbacks_(nullptr),
point_client_data_(nullptr),
imu_data_callbacks_(nullptr),
imu_client_data_(nullptr) {
}
DataHandler& DataHandler::GetInstance() {
static DataHandler data_handler;
return data_handler;
}
bool DataHandler::Init() {
LOG_INFO("Data Handler Init Succ.");
return true;
}
void DataHandler::Destory() {
point_data_callbacks_ = nullptr;
point_client_data_ = nullptr;
imu_data_callbacks_ = nullptr;
imu_client_data_ = nullptr;
std::lock_guard<std::mutex> lock(mutex_);
observers_.clear();
}
DataHandler::~DataHandler() {
Destory();
}
void DataHandler::Handle(const uint8_t dev_type, const uint32_t handle, uint8_t *buf, uint32_t buf_size) {
LivoxLidarEthernetPacket *lidar_data = (LivoxLidarEthernetPacket *)buf;
if (lidar_data == NULL) {
return;
}
if (lidar_data->data_type == kLivoxLidarImuData) {
if (imu_data_callbacks_) {
imu_data_callbacks_(handle, dev_type, lidar_data, imu_client_data_);
}
} else {
if (point_data_callbacks_) {
point_data_callbacks_(handle, dev_type, lidar_data, point_client_data_);
}
}
{
std::lock_guard<std::mutex> lock(mutex_);
for (const auto& observer : observers_) {
auto callback = observer.second.first;
auto client_data = observer.second.second;
if (callback) {
callback(handle, dev_type, lidar_data, client_data);
}
}
}
}
uint16_t DataHandler::AddPointCloudObserver(const DataCallback &cb, void *client_data) {
uint16_t observer_id = GenerateObserverId();
{
std::lock_guard<std::mutex> lock(mutex_);
observers_[observer_id] = std::make_pair(cb, client_data);
}
return observer_id;
}
void DataHandler::RemovePointCloudObserver(uint16_t id) {
std::lock_guard<std::mutex> lock(mutex_);
if (observers_.find(id) != observers_.end()) {
observers_.erase(id);
}
}
uint16_t DataHandler::GenerateObserverId() {
static std::atomic<std::uint16_t> observer_id(1);
uint16_t value = observer_id.load();
uint16_t desired = 0;
do {
if (value == UINT16_MAX) {
desired = 1;
} else {
desired = value + 1;
}
} while (!observer_id.compare_exchange_weak(value, desired));
return desired;
}
void DataHandler::SetPointDataCallback(const DataCallback& cb, void *client_data) {
point_data_callbacks_ = cb;
point_client_data_ = client_data;
}
void DataHandler::SetImuDataCallback(const DataCallback& cb, void* client_data) {
imu_data_callbacks_ = cb;
imu_client_data_ = client_data;
}
} // namespace lidar
} // namespace livox
sdk_core/data_handler/data_handler.h
+91
View File
@@ -0,0 +1,91 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_DATA_HANDLER_H_
#define LIVOX_DATA_HANDLER_H_
#include <array>
#include <functional>
#include <memory>
#include <mutex>
#include <utility>
#include "comm/define.h"
#include "base/io_loop.h"
namespace livox {
namespace lidar {
class DataHandler : public noncopyable {
private:
DataHandler();
DataHandler(const DataHandler& other) = delete;
DataHandler& operator=(const DataHandler& other) = delete;
public:
void Destory();
~DataHandler();
static DataHandler& GetInstance();
bool Init();
void Handle(const uint8_t dev_type, const uint32_t handle, uint8_t *buf, uint32_t buf_size);
uint16_t AddPointCloudObserver(const DataCallback &cb, void *client_data);
void RemovePointCloudObserver(uint16_t id);
void SetPointDataCallback(const DataCallback& cb, void *client_data);
void SetImuDataCallback(const DataCallback& cb, void* client_data);
private:
uint16_t GenerateObserverId();
private:
DataCallback point_data_callbacks_;
void* point_client_data_;
DataCallback imu_data_callbacks_;
void* imu_client_data_;
std::map<uint16_t, std::pair<DataCallback, void*>> observers_;
std::mutex mutex_;
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_DATA_HANDLER_H_
sdk_core/debug_point_cloud_handler/debug_point_cloud_handler.cpp
+121
View File
@@ -0,0 +1,121 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "debug_point_cloud_handler.h"
#include "FastCRC/FastCRC.h"
#include "spdlog/fmt/fmt.h"
#include <algorithm>
#include <iostream>
#include <chrono>
namespace livox {
namespace lidar {
DebugPointCloudHandler::DebugPointCloudHandler(std::uint32_t handle, std::string sn, std::uint8_t dev_type, std::string path)
: handle_(handle), sn_(sn), dev_type_(dev_type), file_size_(0) {
if (path.back() == '/') path.pop_back();
file_path_ = path;
}
DebugPointCloudHandler::~DebugPointCloudHandler() {
if (thread_ptr_) {
enable_.store(false);
thread_ptr_->join();
thread_ptr_ = nullptr;
}
}
const std::string GetCurrentSystemTime() {
auto now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
std::tm now_tm;
#ifdef WIN32
localtime_s(&now_tm, &now);
#else
localtime_r(&now, &now_tm);
#endif
char buffer[128];
strftime(buffer, sizeof(buffer), "%Y_%m_%d_%H_%M_%S", &now_tm);
return buffer;
}
bool DebugPointCloudHandler::StoreData(uint8_t* buf, uint32_t buf_size) {
if (buf) {
std::unique_lock<std::mutex> lock(data_mutex_);
std::copy(buf, buf + buf_size, std::back_inserter(data_));
lock.unlock();
cv_.notify_one();
return true;
}
return false;
}
void DebugPointCloudHandler::WriteData() {
while (enable_.load()) {
std::unique_lock<std::mutex> lock(data_mutex_);
cv_.wait_for(lock, std::chrono::seconds(1), [this]{return !data_.empty() || !enable_.load();});
if (!enable_.load()) return;
if (file_size_ >= max_file_size_) {
LOG_WARN("{} file size over 4 GB", file_name_);
return;
}
if (!file_handle_) {
file_handle_ = std::make_shared<std::ofstream>();
file_name_ = fmt::format("lidar_{}_{}.LivoxDebugPointCloudData", handle_, GetCurrentSystemTime());
file_handle_->open(fmt::format("{}/{}",file_path_, file_name_), std::ios::binary);
if (!file_handle_->is_open()) {
LOG_ERROR("filed to open {} path", file_path_);
return;
}
// write file header
FastCRC16 crc_16;
LivoxLidarDebugPointCloudFileHeader file_header;
file_header.file_ver = {0x01};
file_header.dev_type = {dev_type_};
file_header.data_type = {0x01};
memcpy(file_header.sn, sn_.c_str(), sizeof(file_header.sn));
memset(file_header.rsvd, 0, sizeof(file_header.rsvd));
file_header.crc16 = crc_16.ccitt(reinterpret_cast<const uint8_t*>(&file_header),
offsetof(LivoxLidarDebugPointCloudFileHeader, crc16));
file_handle_->write(reinterpret_cast<char*>(&file_header), sizeof(file_header));
file_size_ += sizeof(file_header);
}
file_handle_->write(reinterpret_cast<char*>(data_.data()), data_.size());
file_size_ += data_.size();
data_.resize(0);
}
}
bool DebugPointCloudHandler::Enable(bool enable) {
enable_.store(enable);
if (enable) {
thread_ptr_ = std::make_shared<std::thread>(&DebugPointCloudHandler::WriteData, this);
}
return true;
}
} // namespace lidar
} // namespace livox
sdk_core/debug_point_cloud_handler/debug_point_cloud_handler.h
+75
View File
@@ -0,0 +1,75 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef DEBUG_POINT_CLOUD_HANDLER_H_
#define DEBUG_POINT_CLOUD_HANDLER_H_
#include "base/io_thread.h"
#include "base/noncopyable.h"
#include "base/logging.h"
#include "command_handler/command_impl.h"
#include "comm/define.h"
#include <string>
#include <fstream>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <vector>
namespace livox {
namespace lidar {
class DebugPointCloudHandler {
public:
DebugPointCloudHandler(std::uint32_t handle, std::string sn, std::uint8_t dev_type, std::string path);
~DebugPointCloudHandler();
bool StoreData(uint8_t* buf, uint32_t buf_size);
void WriteData();
bool Enable(bool enable);
private:
std::uint32_t handle_;
std::string sn_;
std::uint8_t dev_type_;
std::uint64_t file_size_{0};
std::string file_path_{""};
std::string file_name_;
std::atomic<bool> enable_{false};
std::vector<uint8_t> data_;
std::mutex data_mutex_;
std::condition_variable cv_;
std::shared_ptr<std::thread> thread_ptr_{nullptr};
std::shared_ptr<std::ofstream> file_handle_{nullptr};
static constexpr uint64_t max_file_size_ = {4ULL * 1024 * 1024 * 1024};
};
} // namespace lidar
} // namespace livox
#endif // DEBUG_POINT_CLOUD_HANDLER_H_
sdk_core/debug_point_cloud_handler/debug_point_cloud_manager.cpp
+63
View File
@@ -0,0 +1,63 @@
#include "debug_point_cloud_manager.h"
#include "spdlog/fmt/fmt.h"
#include <iostream>
#include <cstdio>
namespace livox {
namespace lidar {
DebugPointCloudManager::DebugPointCloudManager() {}
DebugPointCloudManager::~DebugPointCloudManager() {
enable_.store(false);
}
DebugPointCloudManager& DebugPointCloudManager::GetInstance() {
static DebugPointCloudManager singleton;
return singleton;
}
void DebugPointCloudManager::AddDevice(const uint32_t handle, const DetectionData* detection_data) {
if (devices_info_.find(handle) == devices_info_.end()) {
std::string ip = fmt::format("{}.{}.{}.{}", detection_data->lidar_ip[0],
detection_data->lidar_ip[1],
detection_data->lidar_ip[2],
detection_data->lidar_ip[3]);
devices_info_.emplace(handle, LidarDeviceInfo{detection_data->sn, detection_data->dev_type, ip, detection_data->cmd_port});
}
}
void DebugPointCloudManager::Handler(uint32_t handle, uint16_t lidar_port, uint8_t *buf, uint32_t buf_size) {
if (!enable_.load()) return;
if (handlers_.find(handle) == handlers_.end()) {
auto it = devices_info_.find(handle);
if (it != devices_info_.end()) {
handlers_.emplace(handle, std::make_shared<DebugPointCloudHandler>(handle, it->second.sn, it->second.dev_type, path_));
handlers_[handle]->Enable(enable_.load());
}
}
handlers_[handle]->StoreData(buf, buf_size);
}
bool DebugPointCloudManager::Enable(bool enable) {
enable_.store(enable);
for (auto& kv : handlers_) {
kv.second->Enable(enable);
if (!enable) {
kv.second = nullptr;
}
}
return true;
}
bool DebugPointCloudManager::SetStorePath(std::string path) {
if (path.back() == '/') path.pop_back();
path_ = path;
return true;
}
} // namespace lidar
} // namespace livox
sdk_core/debug_point_cloud_handler/debug_point_cloud_manager.h
+71
View File
@@ -0,0 +1,71 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef DEBUG_POINT_CLOUD_MANAGER_H_
#define DEBUG_POINT_CLOUD_MANAGER_H_
#include <functional>
#include <memory>
#include <mutex>
#include <condition_variable>
#include "livox_lidar_def.h"
#include "livox_lidar_api.h"
#include "debug_point_cloud_handler.h"
#include "base/io_thread.h"
#include "base/logging.h"
#include "comm/define.h"
#include "base/network/network_util.h"
namespace livox {
namespace lidar {
class DebugPointCloudManager {
public:
DebugPointCloudManager(const DebugPointCloudManager& other) = delete;
DebugPointCloudManager& operator=(const DebugPointCloudManager& other) = delete;
~DebugPointCloudManager();
void AddDevice(const uint32_t handle, const DetectionData* detection_data);
void Handler(uint32_t handle, uint16_t lidar_port, uint8_t *buf, uint32_t buf_size);
bool Enable(bool enable);
bool SetStorePath(std::string path);
static DebugPointCloudManager& GetInstance();
private:
DebugPointCloudManager();
private:
std::atomic<bool> enable_{false};
std::string path_;
std::map<uint32_t, LidarDeviceInfo> devices_info_;
std::map<uint32_t, std::shared_ptr<DebugPointCloudHandler>> handlers_;
};
} // namespace lidar
} // namespace livox
#endif // DEBUG_POINT_CLOUD_MANAGER_H_
sdk_core/device_manager.cpp
+913
View File
@@ -0,0 +1,913 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifdef _WIN32
#include <winsock2.h>
#else
#include <arpa/inet.h>
#endif
#include "device_manager.h"
#include <iostream>
#include "comm/define.h"
#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"
#include "logger_handler/logger_manager.h"
#include "debug_point_cloud_handler/debug_point_cloud_manager.h"
namespace livox {
namespace lidar {
DeviceManager::DeviceManager()
: sdk_framework_cfg_ptr_(nullptr),
lidars_cfg_ptr_(nullptr),
custom_lidars_cfg_ptr_(nullptr),
lidar_logger_cfg_ptr_(nullptr),
detection_socket_(0),
detection_broadcast_socket_(0),
cmd_io_thread_(nullptr),
data_io_thread_(nullptr),
detection_io_thread_(nullptr),
comm_port_(nullptr),
is_stop_detection_(false),
detection_thread_(nullptr),
is_view_(false),
detection_host_ip_(""),
enable_save_log_(false) {
}
DeviceManager& DeviceManager::GetInstance() {
static DeviceManager device_manager;
return device_manager;
}
bool DeviceManager::Init(const std::string& host_ip, const LivoxLidarLoggerCfgInfo* log_cfg_info) {
is_view_ = true;
detection_host_ip_ = host_ip;
comm_port_.reset(new CommPort());
std::shared_ptr<LivoxLidarLoggerCfg> lidar_logger_cfg_ptr(new LivoxLidarLoggerCfg());
if (log_cfg_info != nullptr) {
lidar_logger_cfg_ptr->lidar_log_enable = log_cfg_info->lidar_log_enable;
lidar_logger_cfg_ptr->lidar_log_cache_size = log_cfg_info->lidar_log_cache_size;
lidar_logger_cfg_ptr->lidar_log_path = log_cfg_info->lidar_log_path;
if(!DebugPointCloudManager::GetInstance().SetStorePath(lidar_logger_cfg_ptr->lidar_log_path)) {
LOG_ERROR("Set the debug point cloud file storage path failed.");
return false;
}
}
if (!LoggerManager::GetInstance().Init(lidar_logger_cfg_ptr)) {
LOG_ERROR("Logger manager init failed.");
return false;
}
if (!GeneralCommandHandler::GetInstance().Init(host_ip ,is_view_, this)) {
LOG_ERROR("General command handle init failed.");
return false;
}
if (!DataHandler::GetInstance().Init()) {
LOG_ERROR("Data handle init failed.");
return false;
}
if (!CreateIOThread()) {
LOG_ERROR("Create IO thread failed.");
return false;
}
if (!CreateDetectionChannel()) {
LOG_ERROR("Create detection channel failed.");
return false;
}
detection_thread_ = std::make_shared<std::thread>(&DeviceManager::DetectionLidars, this);
is_stop_detection_.store(false);
return true;
}
bool DeviceManager::Init(std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr,
std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr,
std::shared_ptr<LivoxLidarLoggerCfg> lidar_logger_cfg_ptr,
std::shared_ptr<LivoxLidarSdkFrameworkCfg>& sdk_framework_cfg_ptr) {
is_view_ = false;
lidars_cfg_ptr_ = lidars_cfg_ptr;
custom_lidars_cfg_ptr_ = custom_lidars_cfg_ptr;
lidar_logger_cfg_ptr_ = lidar_logger_cfg_ptr;
sdk_framework_cfg_ptr_ = sdk_framework_cfg_ptr;
if (lidars_cfg_ptr_ && !(lidars_cfg_ptr_->empty())) {
detection_host_ip_ = lidars_cfg_ptr_->at(0).host_net_info.host_ip;
} else if (custom_lidars_cfg_ptr && !(custom_lidars_cfg_ptr->empty())) {
detection_host_ip_ = custom_lidars_cfg_ptr->at(0).host_net_info.host_ip;
} else {
LOG_ERROR("Device manager init failed, can not find cmd host ip.");
return false;
}
comm_port_.reset(new CommPort());
if (!lidar_logger_cfg_ptr) {
LOG_ERROR("lidar_logger_cfg_ptr is nullptr.");
return false;
}
if (!DebugPointCloudManager::GetInstance().SetStorePath(lidar_logger_cfg_ptr->lidar_log_path)) {
LOG_ERROR("Set the debug point cloud file storage path failed.");
return false;
}
if (!LoggerManager::GetInstance().Init(lidar_logger_cfg_ptr)) {
LOG_ERROR("Logger manager init failed.");
return false;
}
if (!GeneralCommandHandler::GetInstance().Init(custom_lidars_cfg_ptr, this)) {
LOG_ERROR("General command handle init failed.");
return false;
}
if (!DataHandler::GetInstance().Init()) {
LOG_ERROR("Data handle init failed.");
return false;
}
GetLidarConfigMap();
if (!CreateIOThread()) {
LOG_ERROR("Create IO thread failed.");
return false;
}
if (!CreateChannel()) {
LOG_ERROR("Create channel failed.");
return false;
}
if (!(lidars_cfg_ptr_->empty()) || !(custom_lidars_cfg_ptr->empty())) {
detection_thread_ = std::make_shared<std::thread>(&DeviceManager::DetectionLidars, this);
is_stop_detection_.store(false);
}
LOG_INFO("Init livox lidars succ.");
return true;
}
void DeviceManager::GetLidarConfigMap() {
for (auto it = lidars_cfg_ptr_->begin(); it != lidars_cfg_ptr_->end(); ++it) {
const LivoxLidarCfg& lidar_cfg = *it;
type_lidars_cfg_map_[lidar_cfg.device_type] = lidar_cfg;
}
for (auto it = custom_lidars_cfg_ptr_->begin(); it != custom_lidars_cfg_ptr_->end(); ++it) {
const LivoxLidarCfg& lidar_cfg = *it;
uint32_t lidar_ip = inet_addr(lidar_cfg.lidar_net_info.lidar_ipaddr.c_str());
custom_lidars_cfg_map_[lidar_ip] = lidar_cfg;
}
}
bool DeviceManager::CreateIOThread() {
if (!CreateDetectionIOThread()) {
LOG_ERROR("Device manager init failed, create detection io thread failed.");
return false;
}
if (!CreateCommandIOThread()) {
LOG_ERROR("Device manager init failed, create comman io thread failed.");
return false;
}
if (!CreateDataIOThread()) {
LOG_ERROR("Device manager init failed, create data io thread failed.");
return false;
}
return true;
}
bool DeviceManager::CreateDetectionIOThread() {
detection_io_thread_ = std::make_shared<IOThread>();
if (detection_io_thread_ == nullptr || !(detection_io_thread_->Init(true, false))) {
LOG_ERROR("Create command io thread failed, thread_ptr is nullptr or thread init failed");
return false;
}
return detection_io_thread_->Start();
}
bool DeviceManager::CreateCommandIOThread() {
cmd_io_thread_ = std::make_shared<IOThread>();
if (cmd_io_thread_ == nullptr || !(cmd_io_thread_->Init(true, false))) {
LOG_ERROR("Create command io thread failed, thread_ptr is nullptr or thread init failed");
return false;
}
return cmd_io_thread_->Start();
}
bool DeviceManager::CreateDataIOThread() {
data_io_thread_ = std::make_shared<IOThread>();
if (data_io_thread_ == nullptr || !(data_io_thread_->Init(true, false))) {
LOG_ERROR("Create command io thread failed, thread_ptr is nullptr or thread init failed");
return false;
}
return data_io_thread_->Start();
}
bool DeviceManager::CreateChannel() {
if (!CreateDetectionChannel()) {
LOG_ERROR("Create detection channel failed.");
return false;
}
for (auto it = custom_lidars_cfg_ptr_->begin(); it != custom_lidars_cfg_ptr_->end(); ++it) {
const HostNetInfo& host_net_info = it->host_net_info;
if (!CreateDataChannel(host_net_info)) {
LOG_ERROR("Create data channel failed.");
return false;
}
if (!CreateCommandChannel(it->device_type, host_net_info)) {
LOG_ERROR("Create command channel failed.");
return false;
}
}
return true;
}
bool DeviceManager::CreateDetectionChannel() {
#ifdef WIN32
#else
detection_broadcast_socket_ = util::CreateSocket(kDetectionPort, true, true, true, "255.255.255.255", "");
if (detection_broadcast_socket_ < 0) {
LOG_ERROR("Create detection broadcast socket failed.");
return false;
}
detection_io_thread_->GetLoop().lock()->AddDelegate(detection_broadcast_socket_, this, nullptr);
#endif
std::string key = detection_host_ip_ + ":" + std::to_string(kDetectionPort);
detection_socket_ = util::CreateSocket(kDetectionPort, true, true, true, detection_host_ip_, "");
if (detection_socket_ < 0) {
LOG_ERROR("Create detection socket failed.");
return false;
}
detection_io_thread_->GetLoop().lock()->AddDelegate(detection_socket_, this, nullptr);
channel_info_[key] = detection_socket_;
if (custom_command_channel_.find(key) == custom_command_channel_.end()) {
custom_command_channel_[key] = detection_socket_;
}
return true;
}
bool DeviceManager::CreateDataChannel(const HostNetInfo& host_net_info) {
if (!CreateDataSocketAndAddDelegate(host_net_info.host_ip, host_net_info.point_data_port, host_net_info.multicast_ip)) {
LOG_ERROR("Create socket and add delegate failed.");
return false;
}
if (!CreateDataSocketAndAddDelegate(host_net_info.host_ip, host_net_info.imu_data_port, host_net_info.multicast_ip)) {
LOG_ERROR("Create socket and add delegate failed.");
return false;
}
if (!CreateDataSocketAndAddDelegate(host_net_info.host_ip, kHostDebugPointCloudPort, host_net_info.multicast_ip)) {
LOG_ERROR("Create debug point cloud socket and add delegate failed.");
return false;
}
return true;
}
bool DeviceManager::CreateCommandChannel(const uint8_t dev_type, const HostNetInfo& host_net_info) {
if (sdk_framework_cfg_ptr_->master_sdk) {
if (!CreateCmdSocketAndAddDelegate(dev_type, host_net_info.host_ip, host_net_info.cmd_data_port, kCmd)) {
LOG_ERROR("Create socket and add delegate failed.");
return false;
}
}
if (!CreateCmdSocketAndAddDelegate(dev_type, host_net_info.host_ip, host_net_info.push_msg_port, kPush)) {
LOG_ERROR("Create socket and add delegate failed.");
return false;
}
if (dev_type == kLivoxLidarTypePA) {
//if (!CreateCmdSocketAndAddDelegate(dev_type, host_net_info.push_msg_ip, kPaHostFaultPort, is_custom)) {
if (!CreateCmdSocketAndAddDelegate(dev_type, host_net_info.host_ip, kPaHostFaultPort, kFault)) {
LOG_ERROR("Create socket and add delegate failed.");
return false;
}
}
#ifdef WIN32
#else
if (dev_type == kLivoxLidarTypeMid360 || dev_type == kLivoxLidarTypeMid360s) {
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_->GetLoop().lock()->AddDelegate(broadcast_socket, this, nullptr);
}
#endif
if (LoggerManager::GetInstance().GetLogEnable()) {
if (!CreateCmdSocketAndAddDelegate(dev_type, host_net_info.host_ip, host_net_info.log_data_port, kLog)) {
LOG_ERROR("Create socket and add delegate failed.");
return false;
}
}
return true;
}
bool DeviceManager::CreateCmdSocketAndAddDelegate(const uint8_t dev_type, const std::string& host_ip,
const uint16_t port, const HostSocketType type) {
if (host_ip.empty() || port == 0 || port == kLogPort) {
return true;
}
std::string key = host_ip + ":" + std::to_string(port);
if (channel_info_.find(key) != channel_info_.end()) {
if (custom_command_channel_.find(key) == custom_command_channel_.end()) {
custom_command_channel_[key] = channel_info_[key];
}
return true;
}
socket_t sock = -1;
if (host_ip == "local") {
sock = util::CreateSocket(port, true, true, true, "", "");
} else {
sock = util::CreateSocket(port, true, true, true, host_ip, "");
}
if (sock < 0) {
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);
custom_command_channel_[key] = sock;
cmd_io_thread_->GetLoop().lock()->AddDelegate(sock, this, nullptr);
return true;
}
bool DeviceManager::CreateDataSocketAndAddDelegate(const std::string& host_ip, const uint16_t port, const std::string& multicast_ip) {
if (host_ip.empty() || port == 0 || port == kLogPort || port == kDetectionPort) {
return true;
}
std::string key = host_ip + ":" + std::to_string(port);
if (channel_info_.find(key) != channel_info_.end()) {
return true;
}
socket_t sock = -1;
if (host_ip == "local") {
sock = util::CreateSocket(port, true, true, false, "", multicast_ip);
} else {
sock = util::CreateSocket(port, true, true, false, host_ip, multicast_ip);
}
if (sock < 0) {
LOG_ERROR("Add command channel faileld, can not create socket, the ip {} port {} ", host_ip.c_str(), port);
return false;
}
socket_vec_.push_back(sock);
channel_info_[key] = sock;
data_channel_.insert(sock);
data_io_thread_->GetLoop().lock()->AddDelegate(sock, this, nullptr);
return true;
}
void DeviceManager::DetectionLidars() {
while (!is_stop_detection_) {
Detection();
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}
void DeviceManager::Detection() {
uint8_t req_buff[kMaxCommandBufferSize] = {0};
CommPacket packet;
packet.protocol = kLidarSdk;
packet.version = kSdkVer;
packet.seq_num = GenerateSeq::GetSeq();
packet.cmd_id = kCommandIDLidarSearch;
packet.cmd_type = kCommandTypeCmd;
packet.sender_type = kHostSend;
packet.data = req_buff;
packet.data_len = 0;
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
int size = 0;
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, packet);
struct sockaddr_in servaddr;
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = inet_addr("255.255.255.255");
servaddr.sin_port = htons(kDetectionPort);
int byte_send = sendto(detection_socket_, (const char*)buf.data(), size, 0,
(const struct sockaddr *) &servaddr, sizeof(servaddr));
if (byte_send < 0) {
LOG_INFO("Detection lidars failed, Send to lidar failed.");
}
}
void DeviceManager::OnData(socket_t sock, void *client_data) {
struct sockaddr addr;
int addrlen = sizeof(addr);
std::unique_ptr<char[]> buf = nullptr;
if (buf.get() == NULL) {
buf.reset(new char[kMaxBufferSize]);
}
int size = util::RecvFrom(sock, reinterpret_cast<char *>(buf.get()), kMaxBufferSize, 0, &addr, &addrlen);
if (size <= 0) {
return;
}
uint32_t handle = ((struct sockaddr_in *)&addr)->sin_addr.s_addr;
uint16_t port = ntohs(((struct sockaddr_in *)&addr)->sin_port);
struct in_addr tmp_addr;
tmp_addr.s_addr = handle;
std::string lidar_ip = inet_ntoa(tmp_addr);
if (lidar_ip == detection_host_ip_) {
return;
}
if (port == kMid360LidarDebugPointCloudPort || port == kHAPDebugPointCloudPort) {
DebugPointCloudManager::GetInstance().Handler(handle, port, (uint8_t*)(buf.get()), size);
}
if (port == kHAPLogPort || port == kPaLidarLogPort || port == kMid360LidarLogPort) {
LoggerManager::GetInstance().Handler(handle, port, (uint8_t*)(buf.get()), size);
}
if (is_view_) {
std::shared_ptr<ViewLidarIpInfo> view_lidar_info_ptr = nullptr;
{
std::lock_guard<std::mutex> lock(view_lidars_info_mutex_);
if (view_lidars_info_.find(handle) != view_lidars_info_.end()) {
view_lidar_info_ptr = view_lidars_info_[handle];
}
}
if (view_lidar_info_ptr != nullptr) {
if (port == view_lidar_info_ptr->lidar_point_port || port == view_lidar_info_ptr->lidar_imu_data_port) {
DataHandler::GetInstance().Handle(view_lidar_info_ptr->dev_type, handle, (uint8_t*)(buf.get()), size);
} else {
GeneralCommandHandler::GetInstance().Handler(view_lidar_info_ptr->dev_type, handle, port, (uint8_t*)(buf.get()), size);
}
} else {
GeneralCommandHandler::GetInstance().Handler(handle, port, (uint8_t*)(buf.get()), size);
}
return;
}
if (custom_lidars_cfg_map_.find(handle) != custom_lidars_cfg_map_.end()) {
const LivoxLidarCfg& lidar_cfg = custom_lidars_cfg_map_[handle];
if (port == lidar_cfg.lidar_net_info.imu_data_port || port == lidar_cfg.lidar_net_info.point_data_port) {
DataHandler::GetInstance().Handle(lidar_cfg.device_type, handle, (uint8_t*)(buf.get()), size);
return;
}
if (port == kDetectionPort || port == lidar_cfg.lidar_net_info.cmd_data_port || port == lidar_cfg.lidar_net_info.push_msg_port ||
port == lidar_cfg.lidar_net_info.log_data_port || port == kPaLidarFaultPort) {
GeneralCommandHandler::GetInstance().Handler(lidar_cfg.device_type, handle, port, (uint8_t*)(buf.get()), size);
return;
}
return;
}
// parse the device type info from config_ptr and add to custom_lidars_cfg_map_
if (port != kDetectionPort) {
return;
}
CommPacket packet;
memset(&packet, 0, sizeof(packet));
if (!(comm_port_->ParseCommStream((uint8_t*)(buf.get()), size, &packet))) {
LOG_INFO("Parse Command Stream failed.");
return;
}
if (packet.cmd_id != kCommandIDLidarSearch) {
return;
}
if (packet.data == nullptr || packet.data_len == 0) {
return;
}
DetectionData* detection_data = (DetectionData*)(packet.data);
if (detection_data->ret_code != 0) {
return;
}
if (type_lidars_cfg_map_.find(detection_data->dev_type) == type_lidars_cfg_map_.end()) {
return;
}
LivoxLidarCfg& lidar_cfg = type_lidars_cfg_map_[detection_data->dev_type];
struct in_addr binary_ip;
binary_ip.s_addr = handle;
lidar_cfg.lidar_net_info.lidar_ipaddr = inet_ntoa(binary_ip);
custom_lidars_cfg_map_[handle] = lidar_cfg;
custom_lidars_cfg_ptr_->push_back(lidar_cfg);
GeneralCommandHandler::GetInstance().Init(custom_lidars_cfg_ptr_, this);
GeneralCommandHandler::GetInstance().CreateCommandHandler(detection_data->dev_type);
for (auto it = custom_lidars_cfg_ptr_->begin(); it != custom_lidars_cfg_ptr_->end(); ++it) {
const HostNetInfo& host_net_info = it->host_net_info;
if (!CreateDataChannel(host_net_info)) {
LOG_ERROR("Create data channel failed.");
return;
}
if (!CreateCommandChannel(it->device_type, host_net_info)) {
LOG_ERROR("Create command channel failed.");
return;
}
}
return;
}
void DeviceManager::HandleDetectionData(uint32_t handle, DetectionData* detection_data, bool is_get_loader_mode, bool is_load_mode) {
if (handle == 0 || detection_data == nullptr) {
return;
}
if (is_view_) {
{
std::lock_guard<std::mutex> lock(view_device_mutex_);
if (view_devices_.find(handle) == view_devices_.end()) {
ViewDevice& view_device = view_devices_[handle];
view_device.handle = handle;
view_device.dev_type = detection_data->dev_type;
view_device.cmd_port = detection_data->cmd_port;
view_device.is_get.store(false);
view_device.is_set.store(false);
}
}
const ViewDevice& view_device = view_devices_[handle];
if (!view_device.is_get.load() && is_get_loader_mode && !is_load_mode) {
GetLivoxLidarInternalInfo(handle);
} else if (is_get_loader_mode && !is_load_mode) {
if (!view_device.is_set.load() && is_get_loader_mode && !is_load_mode) {
std::shared_ptr<ViewLidarIpInfo> view_lidar_info_ptr = nullptr;
{
std::lock_guard<std::mutex> lock(view_lidars_info_mutex_);
view_lidar_info_ptr = view_lidars_info_[handle];
}
if (view_lidar_info_ptr == nullptr) {
LOG_ERROR("Update view lidar cfg failed, can not find lidar info, the handle:{}", handle);
return;
}
GeneralCommandHandler::GetInstance().UpdateLidarCfg(*view_lidar_info_ptr);
}
}
if (is_get_loader_mode && is_load_mode) {
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
}
return;
}
std::lock_guard<std::mutex> lock(lidars_dev_type_mutex_);
if (lidars_dev_type_.find(handle) != lidars_dev_type_.end()) {
uint16_t dev_type = lidars_dev_type_[handle];
if (dev_type != detection_data->dev_type) {
LOG_ERROR("The lidar of handle:{} dev_type is error, the dev_type1:{}, the dev_type2:{}",
handle, dev_type, detection_data->dev_type);
return;
}
if (is_get_loader_mode && is_load_mode) {
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
}
return;
}
lidars_dev_type_[handle] = detection_data->dev_type;
if (is_get_loader_mode && is_load_mode) {
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
}
}
void DeviceManager::GetLivoxLidarInternalInfo(const uint32_t handle) {
CommandImpl::QueryLivoxLidarInternalInfo(handle, DeviceManager::GetLivoxLidarInternalInfoCallback, this);
}
void DeviceManager::GetLivoxLidarInternalInfoCallback(livox_status status, uint32_t handle,
LivoxLidarDiagInternalInfoResponse* response, void* client_data) {
if (client_data == nullptr) {
LOG_ERROR("Get livox lidar internal info failed, client data is nullptr.");
return;
}
if (status != kLivoxLidarStatusSuccess) {
LOG_ERROR("Get livox lidar internal info failed, the status:{}", status);
return;
}
DeviceManager* self = (DeviceManager*)(client_data);
self->AddViewLidar(handle, response);
}
void DeviceManager::AddViewLidar(const uint32_t handle, LivoxLidarDiagInternalInfoResponse* response) {
if (response == nullptr) {
return;
}
if (response->ret_code != 0) {
LOG_ERROR("Get livox lidar internal info failed, the ret_code:{}", response->ret_code);
return;
}
{
std::lock_guard<std::mutex> lock(view_device_mutex_);
if (view_devices_.find(handle) == view_devices_.end()) {
LOG_ERROR("Add view lidar failed, can not get cmd port, the handle:{}", handle);
return;
}
}
ViewDevice& view_device = view_devices_[handle];
if (view_device.is_get.load()) {
return;
}
std::shared_ptr<ViewLidarIpInfo> view_lidar_info_ptr(new ViewLidarIpInfo());
view_lidar_info_ptr->handle = handle;
view_lidar_info_ptr->dev_type = view_device.dev_type;
view_lidar_info_ptr->lidar_cmd_port = view_device.cmd_port;
view_lidar_info_ptr->host_ip = detection_host_ip_;
uint16_t off = 0;
for (uint8_t i = 0; i < response->param_num; ++i) {
LivoxLidarKeyValueParam* kv = (LivoxLidarKeyValueParam*)&response->data[off];
if (kv->key == kKeyLidarPointDataHostIpCfg) {
memcpy(&(view_lidar_info_ptr->host_point_port), &(kv->value[4]), sizeof(uint16_t));
memcpy(&(view_lidar_info_ptr->lidar_point_port), &(kv->value[6]), sizeof(uint16_t));
} else if (kv->key == kKeyLidarImuHostIpCfg) {
memcpy(&(view_lidar_info_ptr->host_imu_data_port), &(kv->value[4]), sizeof(uint16_t));
memcpy(&(view_lidar_info_ptr->lidar_imu_data_port), &(kv->value[6]), sizeof(uint16_t));
}
off += sizeof(uint16_t) * 2;
off += kv->length;
}
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;
}
if (view_lidar_info_ptr->dev_type == kLivoxLidarTypeMid360s) {
view_lidar_info_ptr->lidar_point_port = kMid360sLidarPointCloudPort;
view_lidar_info_ptr->lidar_imu_data_port = kMid360sLidarImuDataPort;
}
CreateViewDataChannel(*view_lidar_info_ptr);
{
std::lock_guard<std::mutex> lock(view_lidars_info_mutex_);
view_lidars_info_[handle] = view_lidar_info_ptr;
}
view_device.is_get.store(true);
GeneralCommandHandler::GetInstance().UpdateLidarCfg(*view_lidar_info_ptr);
}
void DeviceManager::CreateViewDataChannel(const ViewLidarIpInfo& view_lidar_info) {
std::string point_key = view_lidar_info.host_ip + ":" + std::to_string(view_lidar_info.host_point_port);
if (channel_info_.find(point_key) == channel_info_.end()) {
socket_t sock = -1;
sock = util::CreateSocket(view_lidar_info.host_point_port, true, true, true, view_lidar_info.host_ip, "");
if (sock < 0) {
LOG_ERROR("Create View point data channel faileld, can not create socket, the ip {} port {} ",
view_lidar_info.host_ip.c_str(), view_lidar_info.host_point_port);
return;
}
socket_vec_.push_back(sock);
channel_info_[point_key] = sock;
data_io_thread_->GetLoop().lock()->AddDelegate(sock, this, nullptr);
}
std::string imu_key = view_lidar_info.host_ip + ":" + std::to_string(view_lidar_info.host_imu_data_port);
if (channel_info_.find(imu_key) == channel_info_.end()) {
socket_t sock = -1;
sock = util::CreateSocket(view_lidar_info.host_imu_data_port, true, true, true, view_lidar_info.host_ip, "");
if (sock < 0) {
LOG_ERROR("Create View point data channel faileld, can not create socket, the ip {} port {} ",
view_lidar_info.host_ip.c_str(), view_lidar_info.host_imu_data_port);
return;
}
socket_vec_.push_back(sock);
channel_info_[imu_key] = sock;
data_io_thread_->GetLoop().lock()->AddDelegate(sock, this, nullptr);
}
}
void DeviceManager::UpdateViewLidarCfgCallback(const uint32_t handle) {
if (is_view_) {
std::lock_guard<std::mutex> lock(view_device_mutex_);
if (view_devices_.find(handle) == view_devices_.end()) {
LOG_ERROR("Device manager change livox lidar faield, can not find the view device info, the handle:{}", handle);
return;
}
view_devices_[handle].is_set.store(true);
}
}
uint8_t DeviceManager::GetDeviceType(const uint32_t handle) {
uint8_t dev_type = 0;
std::lock_guard<std::mutex> lock(lidars_dev_type_mutex_);
if (lidars_dev_type_.find(handle) != lidars_dev_type_.end()) {
dev_type = lidars_dev_type_[handle];
}
return dev_type;
}
void DeviceManager::OnTimer(TimePoint now) {
GeneralCommandHandler::GetInstance().CommandsHandle(now);
}
int DeviceManager::SendCommand(const uint8_t dev_type, const uint32_t handle, const std::vector<uint8_t>& buf,
const int16_t size, const struct sockaddr *addr, socklen_t addrlen) {
socket_t sock = -1;
if (!GetCmdChannel(dev_type, handle, sock)) {
LOG_WARN("Get cmd channel faileld, the lidar handle: {}", handle);
sock = detection_socket_;
}
std::lock_guard<std::mutex> lock(mutex_cmd_channel_);
return sendto(sock, (const char*)buf.data(), size, 0, addr, addrlen);
}
bool DeviceManager::GetCmdChannel(const uint8_t dev_type, const uint32_t handle, socket_t& sock) {
if (custom_lidars_cfg_map_.find(handle) != custom_lidars_cfg_map_.end()) {
const LivoxLidarCfg& lidar_cfg = custom_lidars_cfg_map_[handle];
std::string key = lidar_cfg.host_net_info.host_ip + ":" + std::to_string(lidar_cfg.host_net_info.cmd_data_port);
if (custom_command_channel_.find(key) != custom_command_channel_.end()) {
sock = custom_command_channel_[key];
return true;
}
return false;
}
return false;
}
int DeviceManager::SendLoggerCommand(const uint8_t dev_type, const uint32_t handle, const std::vector<uint8_t>& buf,
const int16_t size, const struct sockaddr *addr, socklen_t addrlen) {
socket_t sock = -1;
if (!GetLoggerCmdChannel(dev_type, handle, sock)) {
sock = detection_socket_;
}
std::lock_guard<std::mutex> lock(mutex_logger_cmd_channel_);
return sendto(sock, (const char*)buf.data(), size, 0, addr, addrlen);
}
bool DeviceManager::GetLoggerCmdChannel(const uint8_t dev_type, const uint32_t handle, socket_t& sock) {
if (custom_lidars_cfg_map_.find(handle) != custom_lidars_cfg_map_.end()) {
const LivoxLidarCfg& lidar_cfg = custom_lidars_cfg_map_[handle];
std::string key = lidar_cfg.host_net_info.host_ip + ":" + std::to_string(lidar_cfg.host_net_info.log_data_port);
if (custom_command_channel_.find(key) != custom_command_channel_.end()) {
sock = custom_command_channel_[key];
return true;
}
return false;
}
return false;
}
void DeviceManager::Destory() {
detection_host_ip_ = "";
if (detection_socket_ > 0) {
detection_io_thread_->GetLoop().lock()->RemoveDelegate(detection_socket_, this);
}
if (detection_broadcast_socket_ > 0) {
detection_io_thread_->GetLoop().lock()->RemoveDelegate(detection_broadcast_socket_, this);
}
for (auto it = command_channel_.begin(); it != command_channel_.end(); ++it) {
socket_t sock = *it;
if (sock > 0) {
cmd_io_thread_->GetLoop().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_->GetLoop().lock()->RemoveDelegate(sock, this);
}
}
for (auto it = data_channel_.begin(); it != data_channel_.end(); ++it) {
socket_t sock = *it;
if (sock > 0) {
data_io_thread_->GetLoop().lock()->RemoveDelegate(sock, this);
}
}
for (socket_t& sock : socket_vec_) {
util::CloseSock(sock);
sock = -1;
}
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();
detection_thread_ = nullptr;
if (detection_socket_ > 0) {
util::CloseSock(detection_socket_);
detection_socket_ = -1;
}
if (detection_broadcast_socket_ > 0) {
util::CloseSock(detection_broadcast_socket_);
detection_broadcast_socket_ = -1;
}
}
lidars_cfg_ptr_ = nullptr;
custom_lidars_cfg_ptr_ = nullptr;
type_lidars_cfg_map_.clear();
custom_lidars_cfg_map_.clear();
channel_info_.clear();
custom_command_channel_.clear();
command_channel_.clear();
data_channel_.clear();
comm_port_.reset(nullptr);
is_stop_detection_.store(true);
{
std::lock_guard<std::mutex> lock(lidars_dev_type_mutex_);
lidars_dev_type_.clear();
}
is_view_ = false;
detection_host_ip_ = "";
{
std::lock_guard<std::mutex> lock(view_device_mutex_);
view_devices_.clear();
}
{
std::lock_guard<std::mutex> lock(view_lidars_info_mutex_);
view_lidars_info_.clear();
}
}
DeviceManager::~DeviceManager() {
Destory();
}
} // namespace lidar
} // namespace livox
sdk_core/device_manager.h
+207
View File
@@ -0,0 +1,207 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_DEVICE_MANAGER_H_
#define LIVOX_DEVICE_MANAGER_H_
#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 <string>
#include <memory>
#include <vector>
#include <set>
#include <map>
#include <mutex>
#include <string.h>
#ifdef WIN32
#include <winsock2.h>
#include <ws2def.h>
#include <ws2tcpip.h>
#else
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#endif // WIN32
namespace livox {
namespace lidar {
static const size_t kMaxBufferSize = 8192;
const uint8_t kSdkVer = 3;
class Protector {};
struct Command {
public:
Command() : packet(), handle(0), lidar_ip(""), time_out(KDefaultTimeOut), cb(nullptr) {}
Command (uint32_t seq_num,
uint16_t cmd_id,
uint8_t cmd_type,
uint8_t sender_type,
uint8_t* data,
uint16_t data_len,
uint32_t handle = 0,
std::string lidar_ip = "",
std::shared_ptr<CommandCallback> cb = nullptr) : packet(), handle(handle), lidar_ip(lidar_ip), time_out(KDefaultTimeOut), cb(cb) {
packet.protocol = kLidarSdk;
packet.version = kSdkVer;
packet.seq_num = seq_num;
packet.cmd_id = cmd_id;
packet.cmd_type = cmd_type;
packet.sender_type = sender_type;
packet.data = data;
packet.data_len = data_len;
}
public:
CommPacket packet;
uint32_t handle;
std::string lidar_ip;
uint32_t time_out;
std::shared_ptr<CommandCallback> cb;
};
class DeviceManager : public IOLoop::IOLoopDelegate {
private:
DeviceManager();
DeviceManager(const DeviceManager& other) = delete;
DeviceManager& operator=(const DeviceManager& other) = delete;
public:
typedef std::chrono::steady_clock::time_point TimePoint;
void Destory();
~DeviceManager();
static DeviceManager& GetInstance();
bool Init(const std::string& host_ip, const LivoxLidarLoggerCfgInfo* log_cfg_info);
bool Init(std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr,
std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr,
std::shared_ptr<LivoxLidarLoggerCfg> lidar_logger_cfg_ptr,
std::shared_ptr<LivoxLidarSdkFrameworkCfg>& sdk_framework_cfg_ptr);
void HandleDetectionData(uint32_t handle, DetectionData* detection_data, bool is_get_loader_mode, bool is_load_mode);
int SendCommand(const uint8_t dev_type, const uint32_t handle, const std::vector<uint8_t>& buf,
const int16_t size, const struct sockaddr *addr, socklen_t addrlen);
int SendLoggerCommand(const uint8_t dev_type, const uint32_t handle, const std::vector<uint8_t>& buf,
const int16_t size, const struct sockaddr *addr, socklen_t addrlen);
static void GetLivoxLidarInternalInfoCallback(livox_status status, uint32_t handle,
LivoxLidarDiagInternalInfoResponse* response, void* client_data);
void UpdateViewLidarCfgCallback(const uint32_t handle);
void OnData(socket_t sock, void *);
void OnTimer(TimePoint now);
std::shared_ptr<LivoxLidarSdkFrameworkCfg> sdk_framework_cfg_ptr_;
private:
void GetLivoxLidarInternalInfo(const uint32_t handle);
void AddViewLidar(const uint32_t handle, LivoxLidarDiagInternalInfoResponse* response);
void CreateViewDataChannel(const ViewLidarIpInfo& view_lidar_info);
void GetLidarConfigMap();
void InitDevTypeTable(const LivoxLidarCfg& lidar_cfg);
bool CreateIOThread();
bool CreateDetectionIOThread();
bool CreateCommandIOThread();
bool CreateDataIOThread();
bool CreateChannel();
bool CreateDetectionChannel();
bool CreateDataChannel(const HostNetInfo& host_net_info);
bool CreateCommandChannel(const uint8_t dev_type, const HostNetInfo& host_net_info);
bool CreateCmdSocketAndAddDelegate(const uint8_t dev_type, const std::string& host_ip, const uint16_t port, const HostSocketType type);
bool CreateDataSocketAndAddDelegate(const std::string& host_ip, const uint16_t port, const std::string& multicast_ip);
void DetectionLidars();
void Detection();
uint8_t GetDeviceType(const uint32_t handle);
void IsLidarData(const uint32_t handle, const uint16_t lidar_port, uint8_t& dev_type);
private:
bool GetCmdChannel(const uint8_t dev_type, const uint32_t handle, socket_t& sock);
bool GetLoggerCmdChannel(const uint8_t dev_type, const uint32_t handle, socket_t& sock);
private:
std::shared_ptr<std::vector<LivoxLidarCfg>> lidars_cfg_ptr_;
std::shared_ptr<std::vector<LivoxLidarCfg>> custom_lidars_cfg_ptr_;
std::shared_ptr<LivoxLidarLoggerCfg> lidar_logger_cfg_ptr_;
std::map<uint8_t, LivoxLidarCfg> type_lidars_cfg_map_;
std::map<uint32_t, LivoxLidarCfg> custom_lidars_cfg_map_;
socket_t detection_socket_;
socket_t detection_broadcast_socket_;
//socket_t detection_socket2_;
std::vector<socket_t> socket_vec_;
std::mutex mutex_cmd_channel_;
std::mutex mutex_logger_cmd_channel_;
std::map<std::string, socket_t> channel_info_;
std::map<std::string, socket_t> custom_command_channel_;
std::set<socket_t> command_channel_;
std::set<socket_t> data_channel_;
std::vector<socket_t> vec_broadcast_socket_;
std::shared_ptr<IOThread> cmd_io_thread_;
std::shared_ptr<IOThread> data_io_thread_;
std::shared_ptr<IOThread> detection_io_thread_;
std::unique_ptr<CommPort> comm_port_;
std::atomic<bool> is_stop_detection_{false};
std::shared_ptr<std::thread> detection_thread_;
std::mutex lidars_dev_type_mutex_;
std::map<uint32_t, uint16_t> lidars_dev_type_;
bool is_view_;
std::string detection_host_ip_;
std::mutex view_device_mutex_;
std::map<uint32_t, ViewDevice> view_devices_;
std::mutex view_lidars_info_mutex_;
std::map<uint32_t, std::shared_ptr<ViewLidarIpInfo>> view_lidars_info_;
bool enable_save_log_;
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_DEVICE_MANAGER_H_
sdk_core/livox_lidar_sdk.cpp
+337
View File
@@ -0,0 +1,337 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "livox_lidar_api.h"
#include "livox_lidar_def.h"
#include "base/command_callback.h"
#include "base/logging.h"
#include "comm/define.h"
#include "command_handler/command_impl.h"
#include "command_handler/general_command_handler.h"
#include "data_handler/data_handler.h"
#include "logger_handler/logger_manager.h"
#include "upgrade_manager.h"
#include "parse_cfg_file.h"
#include "params_check.h"
#include "device_manager.h"
#ifdef WIN32
#include<winsock2.h>
#endif // WIN32
#include <memory>
#include <vector>
using namespace livox::lidar;
static bool is_initialized = false;
void GetLivoxLidarSdkVer(LivoxLidarSdkVer *version) {
if (version != NULL) {
version->major = LIVOX_LIDAR_SDK_MAJOR_VERSION;
version->minor = LIVOX_LIDAR_SDK_MINOR_VERSION;
version->patch = LIVOX_LIDAR_SDK_PATCH_VERSION;
}
}
bool LivoxLidarSdkInit(const char* path, const char* host_ip, const LivoxLidarLoggerCfgInfo* log_cfg_info) {
if (is_initialized) {
return false;
}
#ifdef WIN32
WORD sockVersion = MAKEWORD(2, 0);
WSADATA wsdata;
if (WSAStartup(sockVersion, &wsdata) != 0) {
return false;
}
#endif // WIN32
InitLogger();
if (path == NULL && host_ip == NULL) {
return false;
}
if (path) {
std::shared_ptr<std::vector<LivoxLidarCfg>> lidars_cfg_ptr = nullptr;
std::shared_ptr<std::vector<LivoxLidarCfg>> custom_lidars_cfg_ptr = nullptr;
std::shared_ptr<LivoxLidarLoggerCfg> lidar_logger_cfg_ptr = nullptr;
std::shared_ptr<LivoxLidarSdkFrameworkCfg> sdk_framework_cfg_ptr = nullptr;
if (!ParseCfgFile(path).Parse(lidars_cfg_ptr, custom_lidars_cfg_ptr, lidar_logger_cfg_ptr, sdk_framework_cfg_ptr)) {
return false;
}
if (!ParamsCheck(lidars_cfg_ptr, custom_lidars_cfg_ptr).Check()) {
return false;
}
if (!DeviceManager::GetInstance().Init(lidars_cfg_ptr, custom_lidars_cfg_ptr, lidar_logger_cfg_ptr, sdk_framework_cfg_ptr)) {
return false;
}
} else {
if (!DeviceManager::GetInstance().Init(host_ip, log_cfg_info)) {
return false;
}
}
is_initialized = true;
return true;
}
void LivoxLidarSdkUninit() {
if (!is_initialized) {
return;
}
LoggerManager::GetInstance().Destory();
// The reason for using WSACleanup() after previous statement is that Destory() still needs to send socket messages.
#ifdef WIN32
WSACleanup();
#endif // WIN32
DeviceManager::GetInstance().Destory();
DataHandler::GetInstance().Destory();
GeneralCommandHandler::GetInstance().Destory();
UninitLogger();
is_initialized = false;
}
bool LivoxLidarSdkStart() {
return true;
}
void SaveLivoxLidarSdkLoggerFile() {
is_save_log_file = true;
}
void DisableLivoxSdkConsoleLogger() {
is_console_log_enable = false;
}
uint16_t LivoxLidarAddPointCloudObserver(LivoxLidarPointCloudObserver cb, void *client_data) {
return DataHandler::GetInstance().AddPointCloudObserver(cb, client_data);
}
void LivoxLidarRemovePointCloudObserver(uint16_t id) {
DataHandler::GetInstance().RemovePointCloudObserver(id);
}
void SetLivoxLidarPointCloudCallBack(LivoxLidarPointCloudCallBack cb, void *client_data) {
DataHandler::GetInstance().SetPointDataCallback(cb, client_data);
}
void LivoxLidarAddCmdObserver(LivoxLidarCmdObserverCallBack cb, void *client_data) {
GeneralCommandHandler::GetInstance().LivoxLidarAddCmdObserver(cb, client_data);
}
void LivoxLidarRemoveCmdObserver() {
GeneralCommandHandler::GetInstance().LivoxLidarRemoveCmdObserver();
}
void SetLivoxLidarImuDataCallback(LivoxLidarImuDataCallback cb, void* client_data) {
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 CommandImpl::QueryLivoxLidarInternalInfo(handle, cb, client_data);
}
livox_status QueryLivoxLidarFwType(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data) {
return CommandImpl::QueryLivoxLidarFwType(handle, cb, client_data);
}
livox_status QueryLivoxLidarFirmwareVer(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data) {
return CommandImpl::QueryLivoxLidarFirmwareVer(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 CommandImpl::SetLivoxLidarPclDataType(handle, data_type, cb, client_data);
}
livox_status SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* 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 CommandImpl::SetLivoxLidarDualEmit(handle, enable, cb, client_data);
}
livox_status EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::EnableLivoxLidarPointSend(handle, cb, client_data);
}
livox_status DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::DisableLivoxLidarPointSend(handle, cb, client_data);
}
livox_status SetLivoxLidarIp(uint32_t handle, LivoxLidarIpInfo* ip_config,
LivoxLidarAsyncControlCallback cb, void* 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 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 CommandImpl::SetLivoxLidarImuDataHostIPCfg(handle, *host_imu_ipcfg, cb, client_data);
}
livox_status SetLivoxLidarInstallAttitude(uint32_t handle, LivoxLidarInstallAttitude* install_attitude,
LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::SetLivoxLidarInstallAttitude(handle, *install_attitude, cb, client_data);
}
livox_status SetLivoxLidarFovCfg0(uint32_t handle, FovCfg* fov_cfg0, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::SetLivoxLidarFovCfg0(handle, *fov_cfg0, cb, client_data);
}
livox_status SetLivoxLidarFovCfg1(uint32_t handle, FovCfg* fov_cfg1, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::SetLivoxLidarFovCfg1(handle, *fov_cfg1, cb, client_data);
}
livox_status EnableLivoxLidarFov(uint32_t handle, uint8_t fov_en, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::EnableLivoxLidarFov(handle, fov_en, cb, client_data);
}
livox_status DisableLivoxLidarFov(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::DisableLivoxLidarFov(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 CommandImpl::SetLivoxLidarWorkMode(handle, work_mode, cb, client_data);
}
livox_status EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::EnableLivoxLidarGlassHeat(handle, cb, client_data);
}
livox_status DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::DisableLivoxLidarGlassHeat(handle, cb, client_data);
}
livox_status SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::SetLivoxLidarGlassHeat(handle, glass_heat, cb, client_data);
}
livox_status StartForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::StartForcedHeating(handle, cb, client_data);
}
livox_status StopForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::StopForcedHeating(handle, cb, client_data);
}
livox_status SetLivoxLidarEscMode(uint32_t handle, LivoxLidarEscMode esc_mode, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::SetLivoxLidarEscMode(handle, esc_mode, cb, client_data);
}
livox_status EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::EnableLivoxLidarImuData(handle, cb, client_data);
}
livox_status DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::DisableLivoxLidarImuData(handle, cb, client_data);
}
livox_status EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::EnableLivoxLidarFusaFunciont(handle, cb, client_data);
}
livox_status DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
return CommandImpl::DisableLivoxLidarFusaFunciont(handle, cb, client_data);
}
livox_status SetLivoxLidarDebugPointCloud(uint32_t handle, bool enable, LivoxLidarLoggerCallback cb, void* client_data) {
return CommandImpl::SetLivoxLidarDebugPointCloud(handle, enable, cb, client_data);
}
livox_status SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t rmc_length, LivoxLidarRmcSyncTimeCallBack cb, void* client_data) {
return CommandImpl::SetLivoxLidarRmcSyncTime(handle, rmc, rmc_length, cb, client_data);
}
livox_status SetLivoxLidarWorkModeAfterBoot(const uint32_t handle,const LivoxLidarWorkModeAfterBoot work_mode, LivoxLidarAsyncControlCallback cb, void* client_data){
return CommandImpl::SetLivoxLidarWorkModeAfterBoot(handle, work_mode, cb, client_data);
}
// reset lidar
livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) {
return CommandImpl::LivoxLidarRequestReset(handle, cb, client_data);
}
livox_status LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, void* client_data) {
return CommandImpl::LivoxLidarRequestReboot(handle, cb, client_data);
}
// upgrade
bool SetLivoxLidarUpgradeFirmwarePath(const char* firmware_path) {
return UpgradeManager::GetInstance().SetLivoxLidarUpgradeFirmwarePath(firmware_path);
}
void SetLivoxLidarUpgradeProgressCallback(OnLivoxLidarUpgradeProgressCallback cb, void* client_data) {
UpgradeManager::GetInstance().SetLivoxLidarUpgradeProgressCallback(cb, client_data);
}
void UpgradeLivoxLidars(const uint32_t* handle, const uint8_t lidar_num) {
UpgradeManager::GetInstance().UpgradeLivoxLidars(handle, lidar_num);
}
livox_status LivoxLidarStartLogger(const uint32_t handle, const LivoxLidarLogType log_type, LivoxLidarLoggerCallback cb, void* client_data) {
return LoggerManager::GetInstance().StartLogger(handle, log_type, cb, client_data);
}
livox_status LivoxLidarStopLogger(const uint32_t handle, const LivoxLidarLogType log_type, LivoxLidarLoggerCallback cb, void* client_data) {
return LoggerManager::GetInstance().StopLogger(handle, log_type, cb, client_data);
}
sdk_core/logger_handler/file_manager.cpp
+363
View File
@@ -0,0 +1,363 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "file_manager.h"
#ifdef WIN32
#include <direct.h>
#else
#include <dirent.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#endif
#include <algorithm>
#include <string>
#include <map>
#include "base/logging.h"
namespace livox {
namespace lidar {
constexpr uint16_t kLengthOfTimeInFilename = 19;
constexpr bool kFOk = 0;
#ifdef WIN32
uint64_t GetDirTotalSize(const std::string& dir_name) {
uint64_t total_size = 0;
WIN32_FIND_DATAA data;
HANDLE sh = NULL;
sh = FindFirstFileA((dir_name +"\\*").c_str(), &data);
if (sh == INVALID_HANDLE_VALUE ) {
LOG_ERROR("get directory stat error");
return 0;
}
do {
// skip current and parent
if (std::string(data.cFileName).compare(".") != 0 && std::string(data.cFileName).compare("..") != 0)
{
// if found object is ...
if ((data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) == FILE_ATTRIBUTE_DIRECTORY) {
// directory, then search it recursievly
total_size += GetDirTotalSize(dir_name +"\\"+ data.cFileName);
} else {
// otherwise get object size and add it to directory size
total_size += (__int64) (data.nFileSizeHigh * (MAXDWORD ) + data.nFileSizeLow);
}
}
} while (FindNextFileA(sh, &data));
FindClose(sh);
return total_size;
}
bool GetFileNames(const std::string& dir_name, std::multimap<std::string, std::string> &filenames) {
uint64_t total_size = 0;
WIN32_FIND_DATAA data;
HANDLE sh = NULL;
sh = FindFirstFileA((dir_name +"\\*").c_str(), &data);
if (sh == INVALID_HANDLE_VALUE ) {
LOG_ERROR("get directory stat error");
return false;
}
do {
// skip current and parent
if (std::string(data.cFileName).compare(".") == 0 || std::string(data.cFileName).compare("..") == 0) {
continue;
}
// if found object is ...
if ((data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) == FILE_ATTRIBUTE_DIRECTORY) {
// directory, then search it recursievly
GetFileNames(dir_name +"\\"+ data.cFileName, filenames);
} else {
// otherwise get file name and add it to std::map<std::string, std::string> &filenames
if(!StoreFileName(data.cFileName, filenames)) {
LOG_ERROR("StoreFileName {} failed", data.cFileName);
}
}
} while (FindNextFileA(sh, &data));
FindClose(sh);
return true;
}
bool ChangeHiddenFiles(const std::string& dir_name) {
uint64_t total_size = 0;
WIN32_FIND_DATAA data;
HANDLE sh = NULL;
sh = FindFirstFileA((dir_name +"\\*").c_str(), &data);
if (sh == INVALID_HANDLE_VALUE ) {
LOG_ERROR("get directory stat error");
return 0;
}
do {
// skip current and parent
if (std::string(data.cFileName).compare(".") != 0 && std::string(data.cFileName).compare("..") != 0)
{
// if found object is ...
if ((data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) == FILE_ATTRIBUTE_DIRECTORY) {
// directory, then search it recursievly
ChangeHiddenFiles(dir_name +"\\"+ data.cFileName);
} else {
// otherwise get file name and change it to normal file name
char first_element = '.';
if (data.cFileName[0] != first_element) {
continue;
}
std::string filename(data.cFileName);
if (filename.empty()) {
continue;
}
if(access((dir_name + "\\" + filename).c_str(), kFOk) != 0) {
LOG_WARN("The file to be renamed : {} does not exist ", filename);
continue;
}
std::string file_name_cut = filename.substr(1);
if(access((dir_name + "\\" + file_name_cut).c_str(), kFOk) == 0) {
if (remove((dir_name + "\\" + file_name_cut).c_str()) != 0) {
LOG_WARN("Failed to remove the existing file: {}. errno: {}", file_name_cut, errno);
}
}
if (rename((dir_name + "\\" + filename).c_str(), (dir_name + "\\" + file_name_cut).c_str()) != 0) {
LOG_WARN("Rename hidden file {} failed. errno: {}", filename, errno);
}
}
}
} while (FindNextFileA(sh, &data));
FindClose(sh);
return true;
}
#else
uint64_t GetDirTotalSize(const std::string& dir_name) {
struct stat dir_stat;
if (stat(dir_name.c_str(), &dir_stat) != EXIT_SUCCESS) {
LOG_ERROR("get directory stat error");
return 0;
}
if (S_ISREG(dir_stat.st_mode)) {
return dir_stat.st_size;
}
if (!S_ISDIR(dir_stat.st_mode)) {
LOG_WARN("unknown directory type: {}", dir_name);
return 0;
}
uint64_t total_size = 0;
DIR *dirp = opendir(dir_name.c_str());
if (!dirp) {
LOG_ERROR("opendir: {} failed", dir_name);
return 0;
}
struct dirent *dp = nullptr;
while ((dp = readdir(dirp)) != nullptr) {
// ignore . and ..
if (strcmp(".", dp -> d_name) == EXIT_SUCCESS || strcmp("..", dp -> d_name) == EXIT_SUCCESS) {
continue;
}
std::string sub_dir_name = dir_name;
sub_dir_name.append("/").append(dp -> d_name);
total_size += GetDirTotalSize(sub_dir_name.c_str());
}
closedir(dirp);
return total_size;
}
bool GetFileNames(const std::string& dir_name, std::multimap<std::string, std::string> &filenames) {
DIR *dirp = opendir(dir_name.c_str());
if (!dirp) {
LOG_ERROR("opendir: {} failed", dir_name);
return false;
}
struct dirent *dp = nullptr;
while ((dp = readdir(dirp)) != nullptr) {
if (strcmp(dp->d_name, ".") == 0 ||
strcmp(dp->d_name, "..") == 0 ||
dp->d_type == DT_LNK) {
continue;
}
if (dp->d_type == DT_REG) {
if (dp->d_name[0] == '.') {
continue;
}
if(!StoreFileName(dp->d_name, filenames)) {
LOG_ERROR("StoreFileName {} failed", dp->d_name);
}
} else if (dp->d_type == DT_DIR) {
std::string sub_dir_name = dir_name;
sub_dir_name.append("/").append(dp -> d_name);
GetFileNames(sub_dir_name, filenames);
}
}
closedir(dirp);
return true;
}
bool ChangeHiddenFiles(const std::string& dir_name) {
if (dir_name.empty()) {
return false;
}
DIR *dirp = opendir(dir_name.c_str());
if (!dirp) {
LOG_ERROR("opendir: {} failed", dir_name);
return false;
}
struct dirent *dp = nullptr;
while ((dp = readdir(dirp)) != nullptr) {
if (strcmp(dp->d_name, ".") == 0 || strcmp(dp->d_name, "..") == 0) {
continue;
} else if (dp->d_type == DT_REG) {
char first_element = '.';
if (dp->d_name[0] != first_element) {
continue;
}
std::string filename(dp->d_name);
if (filename.empty()) {
continue;
}
if(access((dir_name + "/" + filename).c_str(), kFOk) != 0) {
LOG_WARN("The file to be renamed : {} does not exist ", filename);
continue;
}
std::string file_name_cut = filename.substr(1);
if(access((dir_name + "/" + file_name_cut).c_str(), kFOk) == 0) {
if (remove((dir_name + "/" + file_name_cut).c_str()) != 0) {
LOG_WARN("Failed to remove the existing file: {}. errno: {}", file_name_cut, errno);
}
}
if (rename((dir_name + "/" + filename).c_str(), (dir_name + "/" + file_name_cut).c_str()) != 0) {
LOG_WARN("Rename hidden file {} failed. errno: {}", filename, errno);
}
} else if (dp->d_type == DT_LNK) {
continue;
} else if (dp->d_type == DT_DIR) {
std::string sub_dir_name = dir_name;
sub_dir_name.append("/");
sub_dir_name.append(dp -> d_name);
ChangeHiddenFiles(sub_dir_name);
}
}
closedir(dirp);
return true;
}
bool DeleteHidFiles(const std::string& dir_name) {
DIR *dirp = opendir(dir_name.c_str());
if (!dirp) {
LOG_ERROR("opendir: {} failed", dir_name);
return false;
}
struct dirent *dp = nullptr;
while ((dp = readdir(dirp)) != nullptr) {
if (strcmp(dp->d_name, ".") == 0 || strcmp(dp->d_name, "..") == 0) {
continue;
} else if (dp->d_type == DT_REG) {
char first_element = '.';
if (dp->d_name[0] != first_element) {
continue;
}
std::string filename(dp->d_name);
remove((dir_name + "/" + filename).c_str());
} else if (dp->d_type == DT_LNK) {
continue;
} else if (dp->d_type == DT_DIR) {
std::string sub_dir_name = dir_name;
sub_dir_name.append("/");
sub_dir_name.append(dp -> d_name);
DeleteHidFiles(sub_dir_name);
}
}
closedir(dirp);
return true;
}
#endif
bool ChangeCurrentFileName(const std::string& dir_name, std::string file_name) {
if (file_name.empty()) {
return false;
}
if (file_name.at(0) != '.') {
return false;
}
if(access((dir_name + "/" + file_name).c_str(), kFOk) != 0) {
LOG_WARN("The file to be renamed : {} does not exist ", file_name);
return false;
}
std::string file_name_cut = file_name.substr(1);
if(access((dir_name + "/" + file_name_cut).c_str(), kFOk) == 0) {
if (remove((dir_name + "/" + file_name_cut).c_str()) != 0) {
LOG_WARN("Failed to remove the existing file: {}. errno: {}", file_name_cut, errno);
}
}
if (rename((dir_name + "/" + file_name).c_str(), (dir_name + "/" + file_name_cut).c_str()) != 0) {
LOG_WARN("Rename hidden file {} failed. errno: {}", file_name, errno);
return false;
}
return true;
}
bool StoreFileName(const char* filename, std::multimap<std::string, std::string> &filenames) {
std::string str_filename(filename);
if (str_filename.empty()) {
return false;
}
std::string record_time = str_filename.substr(0, kLengthOfTimeInFilename);
filenames.insert(std::make_pair(record_time, str_filename));
return true;
}
bool MakeDirecotory(std::string dir) {
int flag = -1;
#ifdef WIN32
flag = mkdir(dir.c_str());
#else
flag = mkdir(dir.c_str(), 0777);
#endif // WIN32
return (flag == 0);
}
bool IsDirectoryExits(std::string dir) {
return access(dir.c_str(), 0) == EXIT_SUCCESS;
}
} // namespace lidar
} // namespace livox
sdk_core/logger_handler/file_manager.h
+50
View File
@@ -0,0 +1,50 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_FILE_MANAGER_
#define LIVOX_FILE_MANAGER_
#include <string>
#include <vector>
#include <map>
namespace livox {
namespace lidar {
uint64_t GetDirTotalSize(const std::string& dir_name);
bool GetFileNames(const std::string& dir_name, std::multimap<std::string, std::string>& files_name);
bool ChangeHiddenFiles(const std::string& dir_name);
bool ChangeCurrentFileName(const std::string& dir_name, std::string file_name);
bool StoreFileName(const char* filename, std::multimap<std::string, std::string>& files_name);
bool DeleteHidFiles(const std::string& dir_name);
bool MakeDirecotory(std::string dir);
bool IsDirectoryExits(std::string dir);
} // namespace lidar
} // namespace livox
#endif // LIVOX_FILE_MANAGER_
sdk_core/logger_handler/logger_handler.cpp
+209
View File
@@ -0,0 +1,209 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "logger_handler.h"
#include <iostream>
#include <sstream>
#include <iomanip>
#include "base/logging.h"
#include "command_handler/command_impl.h"
#include "device_manager.h"
#include "livox_lidar_def.h"
#include "file_manager.h"
namespace livox {
namespace lidar {
std::string GetCurFormatTime() {
std::time_t t = std::time(nullptr);
std::stringstream format_time;
format_time << std::put_time(std::localtime(&t), "%Y-%m-%d_%H-%M-%S");
return format_time.str();
}
void LoggerHandler::Init() {
is_stop_write_.store(false);
thread_ptr_ = std::make_shared<std::thread>(&LoggerHandler::SaveToFile, this);
}
void LoggerHandler::Destory() {
if (thread_ptr_) {
is_stop_write_.store(true);
thread_ptr_->join();
thread_ptr_ = nullptr;
}
for (auto &file: current_files_) {
auto& fp = file.second.fp;
if (fp) {
std::fclose(fp);
fp = nullptr;
}
}
}
void LoggerHandler::SaveToFile() {
while (!is_stop_write_.load()) {
Write();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
void LoggerHandler::StoreLogBag(DeviceLoggerFilePushRequest* req, uint8_t flag) {
LOG_INFO("Transform Data Length : {}", req->data_length);
WriteBuffer write_buff {};
write_buff.log_type = req->log_type;
write_buff.flag = flag;
write_buff.file_index = req->file_index;
write_buff.data_length = req->data_length;
write_buff.trans_index = req->trans_index;
write_buff.data_ptr.reset(new uint8_t[req->data_length], [] (uint8_t * buff) { delete[] buff;});
memcpy(write_buff.data_ptr.get(), req->data, write_buff.data_length);
{
std::lock_guard<std::mutex> lock(queue_mutex_);
queue_.push(std::move(write_buff));
}
}
void LoggerHandler::CreateFile(const WriteBuffer& write_buff) {
std::string current_time_str = GetCurFormatTime();
uint8_t log_type = write_buff.log_type;
log_branch_path_[log_type] = log_root_path_ + (log_root_path_.back() == '/' ? "" : "/") + "type_" + std::to_string(log_type);
if (!IsDirectoryExits(log_branch_path_[log_type])) {
if (!MakeDirecotory(log_branch_path_[log_type])) {
LOG_ERROR("Can't Create Dir {}", log_branch_path_[log_type]);
return;
}
}
// The handling of the four given scenarios has already been included.
if (current_files_[log_type].fp) {
if ((current_files_[log_type].trans_index + 1) != write_buff.trans_index) {
LOG_WARN("The terminal command to end the {}rd log file has been lost.", current_files_[log_type].file_index);
}
std::fclose(current_files_[log_type].fp);
current_files_[log_type].fp = nullptr;
ChangeCurrentFileName(log_branch_path_[log_type], current_files_[log_type].file_name);
}
std::string file_path = log_branch_path_[log_type] + "/" +
"." + current_time_str + "_" + serial_num_ +
"_" + std::to_string(log_type) + "_" + std::to_string(write_buff.file_index) + ".dat";
LOG_INFO("file path : {}", file_path);
current_files_[log_type].fp = std::fopen(file_path.c_str(), "ab");
if (current_files_[log_type].fp) {
std::fwrite(write_buff.data_ptr.get(), 1, write_buff.data_length, current_files_[log_type].fp);
std::fflush(current_files_[log_type].fp);
}
current_files_[log_type].flag = write_buff.flag;
current_files_[log_type].file_index = write_buff.file_index;
current_files_[log_type].trans_index = write_buff.trans_index;
current_files_[log_type].file_name = "." + current_time_str + "_" + serial_num_ +
"_" + std::to_string(log_type) + "_" + std::to_string(write_buff.file_index) + ".dat";
LOG_INFO("Create File index: {}", (int)write_buff.file_index);
}
void LoggerHandler::WriteFile(const WriteBuffer& write_buff) {
uint8_t log_type = write_buff.log_type;
//check file index is equal
if (current_files_[log_type].file_index != write_buff.file_index) {
LOG_WARN("Log Type: {}, File Index error: last file index: {}, current file index: {}",
(int)log_type,
current_files_[log_type].file_index,
write_buff.file_index);
return;
}
if (current_files_[log_type].trans_index + 1 != write_buff.trans_index && write_buff.trans_index != 1) {
LOG_WARN("Log Type: {}, Trans Index error: last trans index: {}, current trans index: {}",
(int)log_type,
current_files_[log_type].trans_index,
write_buff.trans_index);
}
if (current_files_[log_type].fp) {
std::fwrite(write_buff.data_ptr.get(), 1, write_buff.data_length, current_files_[log_type].fp);
std::fflush(current_files_[log_type].fp);
} else {
LOG_ERROR("There is an issue with the lidar firmware, the starting file command is not sent from lidar. trans_index: {}", write_buff.trans_index);
}
current_files_[log_type].flag = write_buff.flag;
current_files_[log_type].trans_index = write_buff.trans_index;
}
void LoggerHandler::StopFile(const WriteBuffer& write_buff) {
uint8_t log_type = write_buff.log_type;
if (current_files_[log_type].flag == static_cast<uint8_t>(Flag::kEndFile) &&
current_files_[log_type].trans_index + 1 != write_buff.trans_index) {
LOG_ERROR("There is an issue with the lidar firmware. Multiple terminal commands to close the log files with discontinuous trans_index.");
}
if (current_files_[log_type].fp) {
std::fclose(current_files_[log_type].fp);
current_files_[log_type].fp = nullptr;
ChangeCurrentFileName(log_branch_path_[log_type], current_files_[log_type].file_name);
}
current_files_[log_type].fp = nullptr;
current_files_[log_type].flag = write_buff.flag;
current_files_[log_type].trans_index = write_buff.trans_index;
}
void LoggerHandler::Write() {
std::queue<WriteBuffer> queue;
{
std::lock_guard<std::mutex> lock(queue_mutex_);
queue.swap(queue_);
}
while (!queue.empty()) {
WriteBuffer& write_buff = queue.front();
if (write_buff.trans_index < current_files_[write_buff.log_type].trans_index &&
write_buff.flag != static_cast<uint8_t>(Flag::kCreateFile)) {
continue;
}
if(write_buff.flag == static_cast<uint8_t>(Flag::kCreateFile)) {
CreateFile(write_buff);
}
if(write_buff.flag == static_cast<uint8_t>(Flag::kEndFile)) {
StopFile(write_buff);
}
if(write_buff.flag == static_cast<uint8_t>(Flag::kTransferData)) {
WriteFile(write_buff);
}
queue.pop();
}
}
} // namespace lidar
} // namespace livox
sdk_core/logger_handler/logger_handler.h
+102
View File
@@ -0,0 +1,102 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_LOGGER_HANDLER_
#define LIVOX_LOGGER_HANDLER_
#include <string>
#include <algorithm>
#include <map>
#include <queue>
#include <mutex>
#include <thread>
#include <stdio.h>
#include "base/io_thread.h"
#include "base/noncopyable.h"
#include "command_handler/command_impl.h"
#include "comm/comm_port.h"
#include "comm/define.h"
namespace livox {
namespace lidar {
class LoggerHandler {
public:
struct CurrentFileInfo {
uint8_t flag {0};
uint8_t file_index {0};
uint32_t trans_index {0};
std::FILE* fp {nullptr};
std::string file_name {""};
};
struct WriteBuffer {
uint8_t log_type;
uint8_t flag {0};
uint8_t file_index {0};
uint16_t data_length {0};
uint32_t trans_index {0};
std::shared_ptr<uint8_t> data_ptr = std::make_shared<uint8_t>(0);
};
public:
explicit LoggerHandler(std::string log_root_path, std::string serial_num) :
log_root_path_(log_root_path),
serial_num_(serial_num),
is_stop_write_(false),
thread_ptr_(nullptr) {
};
~LoggerHandler() {
Destory();
}
void Init();
void Destory();
void StoreLogBag(DeviceLoggerFilePushRequest* req, uint8_t flag);
void CreateFile(const WriteBuffer& write_buff);
void WriteFile(const WriteBuffer& write_buff);
void StopFile(const WriteBuffer& write_buff);
void Write();
void SaveToFile();
private:
std::string log_root_path_;
std::map <uint8_t, std::string> log_branch_path_;
std::string serial_num_;
std::map <uint8_t, CurrentFileInfo> current_files_;
std::mutex queue_mutex_;
std::queue<WriteBuffer> queue_;
std::atomic<bool> is_stop_write_;
std::shared_ptr<std::thread> thread_ptr_;
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_LOGGER_HANDLER_
sdk_core/logger_handler/logger_manager.cpp
+378
View File
@@ -0,0 +1,378 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "logger_manager.h"
#include "file_manager.h"
#include "command_handler/general_command_handler.h"
#include "base/logging.h"
#include "comm/protocol.h"
#include "comm/generate_seq.h"
#include <map>
#include <iomanip>
#include <chrono>
#include <iostream>
#include <condition_variable>
#ifdef WIN32
#include<winsock2.h>
#else
#include <sys/socket.h>
#endif
namespace livox {
namespace lidar {
constexpr uint16_t kMaxExceptionLogCachSizeMb = 200;
constexpr uint16_t kExceptionLogCacheRatio = 1;
constexpr uint16_t kRealtimeLogCacheRatio = 3;
LoggerManager::LoggerManager()
: log_enable_(false),
log_cycle_delete_enable_(false),
log_root_path_("./"),
max_realtimelog_cache_size_(150 * 1024 * 1024),
max_exceptionlog_cache_size_(50 * 1024 * 1024),
comm_port_(nullptr),
cycle_delete_thread_(nullptr),
cond_(false),
is_destroy_(false) {
}
LoggerManager& LoggerManager::GetInstance() {
static LoggerManager logger_manager;
return logger_manager;
}
bool LoggerManager::Init(std::shared_ptr<LivoxLidarLoggerCfg> lidar_logger_cfg_ptr) {
if (lidar_logger_cfg_ptr == nullptr ||
lidar_logger_cfg_ptr->lidar_log_enable == false) {
log_enable_.store(false);
return true;
}
// Do not enable the log when the allocated space is equal to 0 MB and greater than 1000 TB
if (lidar_logger_cfg_ptr->lidar_log_cache_size == 0 || lidar_logger_cfg_ptr->lidar_log_cache_size > 1000000000) {
log_enable_.store(false);
return true;
}
comm_port_.reset(new CommPort());
log_enable_.store(lidar_logger_cfg_ptr->lidar_log_enable);
// Only 200 MB of exception log space will be allocated.
if (lidar_logger_cfg_ptr->lidar_log_cache_size >
kMaxExceptionLogCachSizeMb * (kExceptionLogCacheRatio + kRealtimeLogCacheRatio) / kExceptionLogCacheRatio) {
max_exceptionlog_cache_size_ = kMaxExceptionLogCachSizeMb * 1024 * 1024;
max_realtimelog_cache_size_ = (lidar_logger_cfg_ptr->lidar_log_cache_size - kMaxExceptionLogCachSizeMb) * 1024 * 1024;
} else {
max_realtimelog_cache_size_ = (lidar_logger_cfg_ptr->lidar_log_cache_size * kRealtimeLogCacheRatio /
(kExceptionLogCacheRatio + kRealtimeLogCacheRatio)) * 1024 * 1024;
max_exceptionlog_cache_size_ = (lidar_logger_cfg_ptr->lidar_log_cache_size * kExceptionLogCacheRatio /
(kExceptionLogCacheRatio + kRealtimeLogCacheRatio)) * 1024 * 1024;
}
if (!InitLoggerSavePath(lidar_logger_cfg_ptr->lidar_log_path)) {
LOG_ERROR("Init logger save path failed");
return false;
}
if (!ChangeHiddenFiles(lidar_logger_cfg_ptr->lidar_log_path)) {
LOG_ERROR("Change hidden files to normal files failed");
}
log_cycle_delete_enable_.store(true);
cycle_delete_thread_ = std::make_shared<std::thread>(&LoggerManager::CycleDelete, this);
return true;
}
bool LoggerManager::GetLogEnable() {
return log_enable_.load();
}
bool LoggerManager::InitLoggerSavePath(std::string log_root_path) {
std::string log_root_dir = log_root_path + (log_root_path.back() == '/' ? "" : "/") + "lidar_log/";
if (access(log_root_dir.c_str(), 0) != EXIT_SUCCESS) {
if (!MakeDirecotory(log_root_dir)) {
LOG_ERROR("Can't Create Dir {}", log_root_dir);
return false;
}
}
log_root_path_ = log_root_dir;
return true;
}
void LoggerManager::AddDevice(const uint32_t handle, const DetectionData* detection_data) {
if (devices_info_.find(handle) == devices_info_.end()) {
devices_info_[handle].sn = detection_data->sn;
devices_info_[handle].dev_type = detection_data->dev_type;
std::string lidar_ip = std::to_string(detection_data->lidar_ip[0]) + "." +
std::to_string(detection_data->lidar_ip[1]) + "." +
std::to_string(detection_data->lidar_ip[2]) + "." +
std::to_string(detection_data->lidar_ip[3]);
devices_info_[handle].lidar_ip = lidar_ip;
devices_info_[handle].cmd_port = detection_data->cmd_port;
}
}
void LoggerManager::RemoveDevice(const uint32_t handle) {
if (devices_info_.find(handle) != devices_info_.end()) {
devices_info_.erase(handle);
}
}
livox_status LoggerManager::StartLogger(const uint32_t handle, const LivoxLidarLogType log_type,
LivoxLidarLoggerCallback cb, void* client_data) {
if (log_enable_.load() == false) {
LOG_INFO("Disable logger.");
return kLivoxLidarStatusSuccess;
}
LOG_INFO("Start Logger handler: {}, log_type: {}", handle, log_type);
EnableDeviceLoggerRequest enable_req = {};
enable_req.log_type = static_cast<uint8_t>(log_type);
enable_req.enable = true;
return GeneralCommandHandler::GetInstance().SendLoggerCommand(handle,
kCommandIDLidarCollectionLog, (uint8_t*)&enable_req, sizeof(EnableDeviceLoggerRequest),
MakeCommandCallback<LivoxLidarLoggerResponse>(cb, client_data));
}
void LoggerManager::Handler(uint32_t handle, uint16_t lidar_port, uint8_t *buf, uint32_t buf_size) {
if (!log_enable_.load()) {
return;
}
if (buf == nullptr || buf_size == 0) {
return;
}
CommPacket packet;
memset(&packet, 0, sizeof(packet));
if (!(comm_port_->ParseCommStream((uint8_t*)buf, buf_size, &packet))) {
LOG_INFO("Parse GeneralCommandHandler Command Stream failed.");
return;
}
if (packet.cmd_id != kCommandIDLidarPushLog) {
return;
}
auto data = static_cast<DeviceLoggerFilePushRequest*>((void *)packet.data);
uint8_t flag = data->flag;
if (flag & 1) {
DeviceLoggerFilePushReponse response = {};
response.ret_code = 0x00;
response.log_type = data->log_type;
response.file_index = data->file_index;
response.trans_index = data->trans_index;
GeneralCommandHandler::GetInstance().SendLoggerCommand(handle, kCommandIDLidarPushLog,
(uint8_t*)&response, sizeof(DeviceLoggerFilePushReponse),
MakeCommandCallback<DeviceLoggerFilePushReponse>(nullptr, nullptr)); //Send ACK
}
if (flag & (1 << 1)) {
OnLoggerCreate(handle, data);
return;
}
if (flag & (1 << 2)) {
OnLoggerStopped(handle, data);
return;
}
OnLoggerTransfer(handle, data);
}
void LoggerManager::OnLoggerCreate(const uint32_t handle, DeviceLoggerFilePushRequest* data) {
if (handlers_.find(handle) == handlers_.end()) {
if (devices_info_.find(handle) != devices_info_.end()) {
auto serial_num = devices_info_[handle].sn;
handlers_[handle] = std::make_shared<LoggerHandler>(log_root_path_, serial_num);
handlers_[handle]->Init();
}
}
auto & handler = handlers_[handle];
handler->StoreLogBag(data, static_cast<uint8_t>(Flag::kCreateFile));
}
void LoggerManager::OnLoggerStopped(const uint32_t handle, DeviceLoggerFilePushRequest* data) {
if (handlers_.find(handle) == handlers_.end()) {
LOG_INFO("LogType: {} Stop! File doesn't create", (int)data->log_type);
return;
}
auto & handler = handlers_[handle];
handler->StoreLogBag(data, static_cast<uint8_t>(Flag::kEndFile));
{
std::lock_guard<std::mutex> lock(mutex_);
cond_ = true;
cv_.notify_one();
}
}
void LoggerManager::OnLoggerTransfer(const uint32_t handle, DeviceLoggerFilePushRequest* data) {
if (handlers_.find(handle) == handlers_.end()) {
LOG_ERROR("LogType : {} File doesn't create", (int)data->log_type);
return;
}
auto & handler = handlers_[handle];
handler->StoreLogBag(data, static_cast<uint8_t>(Flag::kTransferData));
}
void LoggerManager::CycleDelete() {
std::string realtime_log_save_path_ = log_root_path_ + (log_root_path_.back() == '/' ? "" : "/") + "type_0";
std::string exception_log_save_path_ = log_root_path_ + (log_root_path_.back() == '/' ? "" : "/") + "type_1";
while (log_cycle_delete_enable_.load()) {
std::unique_lock<std::mutex> lock(mutex_);
cv_.wait_for(lock, std::chrono::seconds(600), [&]{ return cond_; });
if (IsDirectoryExits(realtime_log_save_path_) && (GetDirTotalSize(realtime_log_save_path_) > max_realtimelog_cache_size_)) {
if (!GetFileNames(realtime_log_save_path_, realtime_files_)) {
LOG_ERROR("Can not get filenames in this directory: {}", realtime_log_save_path_);
}
while (GetDirTotalSize(realtime_log_save_path_) > max_realtimelog_cache_size_ && realtime_files_.begin() != realtime_files_.end()) {
remove((realtime_log_save_path_ + "/" + realtime_files_.begin()->second).c_str());
realtime_files_.erase(realtime_files_.begin());
}
realtime_files_.clear();
}
if (IsDirectoryExits(exception_log_save_path_) && GetDirTotalSize(exception_log_save_path_) > max_exceptionlog_cache_size_) {
if (!GetFileNames(exception_log_save_path_, exception_files_)) {
LOG_ERROR("Can not get filenames in this directory: {}", exception_log_save_path_);
}
while (GetDirTotalSize(exception_log_save_path_) > max_exceptionlog_cache_size_ && exception_files_.begin() != exception_files_.end()) {
remove((exception_log_save_path_ + "/" + exception_files_.begin()->second).c_str());
exception_files_.erase(exception_files_.begin());
}
exception_files_.clear();
}
cond_ = false;
}
}
void LoggerManager::StopAllLogger() {
if (!log_enable_.load()) {
return;
}
for (auto it = devices_info_.begin(); it != devices_info_.end(); ++it) {
const uint32_t handle = it->first;
StopLogger(handle, kLivoxLidarRealTimeLog, nullptr, nullptr);
}
}
livox_status LoggerManager::StopLogger(const uint32_t handle, const LivoxLidarLogType log_type,
LivoxLidarLoggerCallback cb, void* client_data) {
LOG_INFO("Stop Logger handler: {}, log_type: {}", handle, log_type);
EnableDeviceLoggerRequest enable_req = {};
enable_req.log_type = log_type;
enable_req.enable = false;
return GeneralCommandHandler::GetInstance().SendLoggerCommand(handle,
kCommandIDLidarCollectionLog, (uint8_t*)&enable_req, sizeof(EnableDeviceLoggerRequest),
MakeCommandCallback<LivoxLidarLoggerResponse>(cb, client_data));
}
void LoggerManager::LoggerStopCallback(livox_status status, uint32_t handle, LivoxLidarLoggerResponse* response, void* client_data) {
if (status != kLivoxLidarStatusSuccess) {
LOG_ERROR("Lidar:{} stop logger failed, the status:{}", handle, status);
if (client_data) {
LoggerManager* manager = static_cast<LoggerManager*>(client_data);
manager->StopLogger(handle, kLivoxLidarRealTimeLog, LoggerManager::LoggerStopCallback, client_data);
}
return;
}
if (response == nullptr) {
LOG_ERROR("Lidar:{} stop logger failed, the response is nullptr.", handle);
LoggerManager* manager = static_cast<LoggerManager*>(client_data);
manager->StopLogger(handle, kLivoxLidarRealTimeLog, LoggerManager::LoggerStopCallback, client_data);
return;
}
if (response->ret_code != 0) {
LOG_ERROR("Lidar:{} stop logger failed, the ret_code:{}.", handle, response->ret_code);
LoggerManager* manager = static_cast<LoggerManager*>(client_data);
manager->StopLogger(handle, kLivoxLidarRealTimeLog, LoggerManager::LoggerStopCallback, client_data);
return;
}
LOG_INFO("The lidar:{} stop logger succ.\n", handle);
LoggerManager* manager = static_cast<LoggerManager*>(client_data);
manager->RemoveDevice(handle);
}
void LoggerManager::Destory() {
if(is_destroy_) {
return;
}
log_cycle_delete_enable_.store(false);
{
std::lock_guard<std::mutex> lock(mutex_);
cond_ = true;
cv_.notify_one();
}
if (cycle_delete_thread_) {
cycle_delete_thread_->join();
cycle_delete_thread_ = nullptr;
}
for (auto it = handlers_.begin(); it != handlers_.end(); ++it) {
it->second->Destory();
}
if (!handlers_.empty()) {
handlers_.clear();
}
StopAllLogger();
if (log_enable_.load()) {
ChangeHiddenFiles(log_root_path_);
}
log_enable_.store(false);
is_destroy_.store(true);
}
LoggerManager::~LoggerManager() {
Destory();
}
} // namespace lidar
} // namespace livox
sdk_core/logger_handler/logger_manager.h
+122
View File
@@ -0,0 +1,122 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_LOGGER_MANAGER_H_
#define LIVOX_LOGGER_MANAGER_H_
#include <functional>
#include <memory>
#include <mutex>
#include <condition_variable>
#include "livox_lidar_def.h"
#include "livox_lidar_api.h"
#include "logger_handler.h"
#include "base/io_thread.h"
#include "comm/define.h"
#include "base/network/network_util.h"
namespace livox {
namespace lidar {
#pragma pack(1)
typedef enum {
kLidarLoggerCreate,
kLidarLoggerStop,
kLidarUnknown
} LogState;
struct LogInfo {
LogInfo() {
this->log_state = LogState::kLidarUnknown;
this->total_log_size = 0;
}
LogState log_state;
uint64_t total_log_size;
};
#pragma pack()
class LoggerManager {
private:
LoggerManager();
LoggerManager(const LoggerManager& other) = delete;
LoggerManager& operator=(const LoggerManager& other) = delete;
public:
typedef std::chrono::steady_clock::time_point TimePoint;
~LoggerManager();
static LoggerManager& GetInstance();
bool Init(std::shared_ptr<LivoxLidarLoggerCfg> lidar_logger_cfg_ptr);
bool GetLogEnable();
void AddDevice(const uint32_t handle, const DetectionData* detection_data);
void RemoveDevice(const uint32_t handle);
void Destory();
livox_status StartLogger(const uint32_t handle, const LivoxLidarLogType log_type, LivoxLidarLoggerCallback cb, void* client_data);
livox_status StopLogger(const uint32_t handle, const LivoxLidarLogType log_type, LivoxLidarLoggerCallback cb, void* client_data);
void Handler(uint32_t handle, uint16_t lidar_port, uint8_t *buf, uint32_t buf_size);
static void LoggerStopCallback(livox_status status, uint32_t handle, LivoxLidarLoggerResponse* response, void* client_data);
private:
bool InitLoggerSavePath(std::string log_root_path);
void CreateDeviceDir(const uint32_t handle, const LidarDeviceInfo& device_info);
void OnLoggerCreate(const uint32_t handle, DeviceLoggerFilePushRequest* data);
void OnLoggerStopped(const uint32_t handle, DeviceLoggerFilePushRequest* data);
void OnLoggerTransfer(const uint32_t handle, DeviceLoggerFilePushRequest* data);
void CycleDelete();
void StopAllLogger();
private:
std::atomic<bool> log_enable_;
std::atomic<bool> log_cycle_delete_enable_;
std::string log_root_path_;
uint64_t max_realtimelog_cache_size_;
uint64_t max_exceptionlog_cache_size_;
std::unique_ptr<CommPort> comm_port_;
std::shared_ptr<std::thread> cycle_delete_thread_;
bool cond_;
std::mutex mutex_;
std::condition_variable cv_;
std::map<uint32_t, LidarDeviceInfo> devices_info_;
std::map<uint32_t, std::shared_ptr<LoggerHandler>> handlers_;
std::multimap<std::string, std::string> realtime_files_;
std::multimap<std::string, std::string> exception_files_;
std::atomic<bool> is_destroy_;
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_LOGGER_MANAGER_H_
sdk_core/params_check.cpp
+212
View File
@@ -0,0 +1,212 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "params_check.h"
#include "livox_lidar_def.h"
#include "comm/define.h"
#include "base/logging.h"
#include <iostream>
#include <string>
#include <memory>
#include <vector>
#include <set>
namespace livox {
namespace lidar {
ParamsCheck::ParamsCheck(std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr,
std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr) : lidars_cfg_ptr_(lidars_cfg_ptr),
custom_lidars_cfg_ptr_(custom_lidars_cfg_ptr) {}
bool ParamsCheck::Check() {
if (lidars_cfg_ptr_ == nullptr && custom_lidars_cfg_ptr_ == nullptr) {
LOG_ERROR("Params check failed, all params is nullptr.");
return false;
}
if (lidars_cfg_ptr_->empty() && custom_lidars_cfg_ptr_->empty()) {
LOG_ERROR("Params check failed, all livox lidars config is empty.");
return false;
}
if (!CheckLidarIp()) {
return false;
}
CheckLidarPort();
if (!CheckLidarMulticastIp()) {
return false;
}
return true;
}
bool ParamsCheck::CheckLidarIp() {
std::set<std::string> lidars_ip;
for (auto it = lidars_cfg_ptr_->begin(); it != lidars_cfg_ptr_->end(); ++it) {
if (it->lidar_net_info.lidar_ipaddr.empty()) {
continue;
}
if (lidars_ip.find(it->lidar_net_info.lidar_ipaddr) != lidars_ip.end()) {
LOG_ERROR("Params check failed, lidar ip conflict, the liar ip:{}", it->lidar_net_info.lidar_ipaddr.c_str());
return false;
}
lidars_ip.insert(it->lidar_net_info.lidar_ipaddr);
}
for (auto it = custom_lidars_cfg_ptr_->begin(); it != custom_lidars_cfg_ptr_->end(); ++it) {
if (it->lidar_net_info.lidar_ipaddr.empty()) {
LOG_ERROR("Params check failed, custom lidar ipaddr is empty.");
return false;
}
if (lidars_ip.find(it->lidar_net_info.lidar_ipaddr) != lidars_ip.end()) {
LOG_ERROR("Params check failed, lidar ip conflict the lidar ip:{}", it->lidar_net_info.lidar_ipaddr.c_str());
return false;
}
lidars_ip.insert(it->lidar_net_info.lidar_ipaddr);
}
return true;
}
bool ParamsCheck::CheckLidarMulticastIp() {
for (auto it = lidars_cfg_ptr_->begin(); it != lidars_cfg_ptr_->end(); ++it) {
if (it->host_net_info.multicast_ip.empty()) {
LOG_INFO("Device type:{} point cloud data and IMU data unicast is enabled.", it->device_type);
continue;
}
std::vector<uint8_t> vec_host_ip;
if (!BuildRequest::IpToU8(it->host_net_info.multicast_ip, ".", vec_host_ip)) {
return false;
}
std::reverse(vec_host_ip.begin(), vec_host_ip.end());
uint32_t net_ip = 0;
memcpy(&net_ip, vec_host_ip.data(), sizeof(uint8_t) * 4);
if (net_ip <= 0xE0000000 || net_ip > 0xEFFFFFFF) {
LOG_ERROR("Params check failed, lidar multicast ip error:{}", it->host_net_info.multicast_ip.c_str());
return false;
}
LOG_INFO("Device type:{} point cloud and IMU data multicast ip:{}", it->device_type, it->host_net_info.multicast_ip.c_str());
}
for (auto it = custom_lidars_cfg_ptr_->begin(); it != custom_lidars_cfg_ptr_->end(); ++it) {
if (it->host_net_info.multicast_ip.empty()) {
LOG_INFO("Lidar ip:{} point cloud data and IMU data unicast is enabled.", it->lidar_net_info.lidar_ipaddr.c_str());
continue;
}
std::vector<uint8_t> vec_host_ip;
if (!BuildRequest::IpToU8(it->host_net_info.multicast_ip, ".", vec_host_ip)) {
return false;
}
std::reverse(vec_host_ip.begin(), vec_host_ip.end());
uint32_t net_ip = 0;
memcpy(&net_ip, vec_host_ip.data(), sizeof(uint8_t) * 4);
if (net_ip <= 0xE0000000 || net_ip > 0xEFFFFFFF) {
LOG_ERROR("Params check failed, lidar multicast ip error:{}", it->host_net_info.multicast_ip.c_str());
return false;
}
LOG_INFO("Lidar ip:{} point cloud and IMU data multicast ip:{}", it->lidar_net_info.lidar_ipaddr.c_str(), it->host_net_info.multicast_ip.c_str());
}
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 && dev_type != kLivoxLidarTypeMid360s) {
return;
}
if (dev_type == kLivoxLidarTypeMid360) {
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;
}
}
if (dev_type == kLivoxLidarTypeMid360s) {
if (lidar_net_info.cmd_data_port != kMid360sLidarCmdPort) {
LOG_ERROR("Mid360s lidar command data port must be {}", kMid360sLidarCmdPort);
lidar_net_info.cmd_data_port = kMid360sLidarCmdPort;
}
if (lidar_net_info.push_msg_port != kMid360sLidarPushMsgPort) {
LOG_ERROR("Mid360s lidar push msg port must be {}", kMid360sLidarPushMsgPort);
lidar_net_info.push_msg_port = kMid360sLidarPushMsgPort;
}
if (lidar_net_info.point_data_port != kMid360sLidarPointCloudPort) {
LOG_ERROR("Mid360s lidar point cloud port must be {}", kMid360sLidarPointCloudPort);
lidar_net_info.point_data_port = kMid360sLidarPointCloudPort;
}
if (lidar_net_info.imu_data_port != kMid360sLidarImuDataPort) {
LOG_ERROR("Mid360s lidar imu data port must be {}", kMid360sLidarImuDataPort);
lidar_net_info.imu_data_port = kMid360sLidarImuDataPort;
}
if (lidar_net_info.log_data_port != kMid360sLidarLogPort) {
LOG_ERROR("Mid360s lidar log port must be {}", kMid360sLidarLogPort);
lidar_net_info.log_data_port = kMid360sLidarLogPort;
}
}
}
} // namespace lidar
} // namespace livox
sdk_core/params_check.h
+61
View File
@@ -0,0 +1,61 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_PARAMS_CHECK_H_
#define LIVOX_PARAMS_CHECK_H_
#include "livox_lidar_def.h"
#include "comm/define.h"
#include <iostream>
#include <string>
#include <memory>
#include <vector>
#include "command_handler/build_request.h"
namespace livox {
namespace lidar {
class ParamsCheck {
public:
ParamsCheck(std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr,
std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr);
bool Check();
private:
bool CheckLidarIp();
bool CheckLidarMulticastIp();
void CheckLidarPort();
void CheckPort(const uint8_t dev_type, LivoxLidarNetInfo& lidar_net_info);
private:
std::shared_ptr<std::vector<LivoxLidarCfg>> lidars_cfg_ptr_;
std::shared_ptr<std::vector<LivoxLidarCfg>> custom_lidars_cfg_ptr_;
};
} // namespace lidar
} // namespace livox
#endif // LIVOX_PARSE_CFG_FILE_H_
sdk_core/parse_cfg_file.cpp
+379
View File
@@ -0,0 +1,379 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "parse_cfg_file.h"
#include "base/logging.h"
#include <map>
#include <string>
namespace livox {
namespace lidar {
const std::map<std::string, LivoxLidarDeviceType> dev_type_map = {
{"HAP", kLivoxLidarTypeIndustrialHAP},
{"MID360", kLivoxLidarTypeMid360},
{"Mid360s", kLivoxLidarTypeMid360s}
};
ParseCfgFile::ParseCfgFile(const std::string& path) : path_(path) {}
bool ParseCfgFile::Parse(std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr,
std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr,
std::shared_ptr<LivoxLidarLoggerCfg>& lidar_logger_cfg_ptr,
std::shared_ptr<LivoxLidarSdkFrameworkCfg>& sdk_framework_cfg_ptr) {
FILE* raw_file = std::fopen(path_.c_str(), "rb");
if (!raw_file) {
LOG_INFO("Parse lidar config failed, can not open json config file!");
}
char read_buffer[32768];
rapidjson::FileReadStream config_file(raw_file, read_buffer, sizeof(read_buffer));
rapidjson::Document doc;
if (doc.ParseStream(config_file).HasParseError()) {
if (raw_file) {
std::fclose(raw_file);
}
LOG_ERROR("Parse lidar config failed, parse the config file has error!");
return false;
}
lidars_cfg_ptr.reset(new std::vector<LivoxLidarCfg>());
custom_lidars_cfg_ptr.reset(new std::vector<LivoxLidarCfg>());
lidar_logger_cfg_ptr.reset(new LivoxLidarLoggerCfg());
sdk_framework_cfg_ptr.reset(new LivoxLidarSdkFrameworkCfg());
if (doc.HasMember("master_sdk")) {
if (doc["master_sdk"].IsBool()) {
sdk_framework_cfg_ptr->master_sdk = doc["master_sdk"].GetBool();
if (sdk_framework_cfg_ptr->master_sdk) {
LOG_INFO("set master/slave sdk to master sdk");
} else {
LOG_INFO("set master/slave sdk to slave sdk");
}
} else {
LOG_ERROR("set master/slave sdk error");
if (raw_file) {
std::fclose(raw_file);
}
return false;
}
} else {
LOG_INFO("set master/slave sdk to master sdk by default");
sdk_framework_cfg_ptr->master_sdk = true;
}
if (doc.HasMember("lidar_log_enable")) {
if (doc["lidar_log_enable"].IsBool()) {
lidar_logger_cfg_ptr->lidar_log_enable = doc["lidar_log_enable"].GetBool();
} else {
LOG_ERROR("Lidar log enable data type is error");
if (raw_file) {
std::fclose(raw_file);
}
return false;
}
if (doc.HasMember("lidar_log_cache_size_MB") && doc["lidar_log_cache_size_MB"].IsUint()) {
lidar_logger_cfg_ptr->lidar_log_cache_size = doc["lidar_log_cache_size_MB"].GetUint();
} else {
LOG_ERROR("Parse json file failed, has not lidar_log_cache_size_MB member or lidar_log_cache_size_MB is uint");
if (raw_file) {
std::fclose(raw_file);
}
return false;
}
if (doc.HasMember("lidar_log_path") && doc["lidar_log_path"].IsString()) {
lidar_logger_cfg_ptr->lidar_log_path = doc["lidar_log_path"].GetString();
} else {
LOG_ERROR("Parse json file failed, has not lidar_log_path member or lidar_log_path is uint");
if (raw_file) {
std::fclose(raw_file);
}
return false;
}
LOG_INFO("Lidar log cfg, lidar_log_enable:{}, lidar_log_cache_size_MB:{}, lidar_log_path:{}",
lidar_logger_cfg_ptr->lidar_log_enable, lidar_logger_cfg_ptr->lidar_log_cache_size,
lidar_logger_cfg_ptr->lidar_log_path.c_str());
} else {
lidar_logger_cfg_ptr->lidar_log_enable = false;
lidar_logger_cfg_ptr->lidar_log_cache_size = 0;
lidar_logger_cfg_ptr->lidar_log_path = "./"; // TODO Executable program path
if (doc.HasMember("lidar_log_path") && doc["lidar_log_path"].IsString()) {
lidar_logger_cfg_ptr->lidar_log_path = doc["lidar_log_path"].GetString();
}
LOG_INFO("Livox lidar logger disable.");
}
if (doc.HasMember("HAP") && doc["HAP"].IsObject()) {
uint8_t device_type = dev_type_map.at("HAP");
const rapidjson::Value &object = doc["HAP"];
if (!ParseLidarCfg(object, device_type, lidars_cfg_ptr, custom_lidars_cfg_ptr)) {
if (raw_file) {
std::fclose(raw_file);
}
return false;
}
}
if (doc.HasMember("MID360") && doc["MID360"].IsObject()) {
uint8_t device_type = dev_type_map.at("MID360");
const rapidjson::Value &object = doc["MID360"];
if (!ParseLidarCfg(object, device_type, lidars_cfg_ptr, custom_lidars_cfg_ptr)) {
if (raw_file) {
std::fclose(raw_file);
}
return false;
}
}
if (doc.HasMember("Mid360s") && doc["Mid360s"].IsObject()) {
uint8_t device_type = dev_type_map.at("Mid360s");
const rapidjson::Value &object = doc["Mid360s"];
if (!ParseLidarCfg(object, device_type, lidars_cfg_ptr, custom_lidars_cfg_ptr)) {
if (raw_file) {
std::fclose(raw_file);
}
return false;
}
}
if (raw_file) {
std::fclose(raw_file);
}
return true;
}
bool ParseCfgFile::ParseLidarCfg(const rapidjson::Value &object, const uint8_t& device_type, std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr, std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr) {
if (object.HasMember("host_net_info") && object["host_net_info"].IsArray()) {
if (!ParseNewLidarCfg(object, device_type, lidars_cfg_ptr, custom_lidars_cfg_ptr)) {
LOG_ERROR("Parse hap lidar new cfg failed.");
return false;
}
} else if (object.HasMember("host_net_info") && object["host_net_info"].IsObject()) {
if (!ParseOldLidarCfg(object, device_type, lidars_cfg_ptr)) {
LOG_ERROR("Parse hap lidar old cfg failed.");
return false;
}
} else {
LOG_ERROR("Parse lidar net info failed, has not host_net_info member or host_net_info is not object or arry.");
return false;
}
return true;
}
bool ParseCfgFile::ParseNewLidarCfg(const rapidjson::Value &object, const uint8_t& device_type, std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr, std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr) {
const rapidjson::Value &host_net_info_object = object["host_net_info"];
size_t config_num = host_net_info_object.Size();
for (size_t i = 0; i < config_num; ++i) {
if (!host_net_info_object[i].HasMember("lidar_ip") || !host_net_info_object[i]["lidar_ip"].IsArray()) {
LivoxLidarCfg lidar_cfg;
if (!ParseTypeLidarCfg(object, host_net_info_object[i], device_type, lidar_cfg)) {
return false;
}
lidars_cfg_ptr->push_back(std::move(lidar_cfg));
continue;
}
const rapidjson::Value &lidar_ip_arr = host_net_info_object[i]["lidar_ip"];
size_t lidar_ip_num = lidar_ip_arr.Size();
for (size_t j = 0; j < lidar_ip_num; ++j) {
LivoxLidarCfg lidar_cfg;
const rapidjson::Value &lidar_ip = lidar_ip_arr[j];
if (!lidar_ip.IsString()) {
LOG_ERROR("Parse lidar ip failed, has not lidar_ip member or lidar_ip is not object.");
return false;
}
lidar_cfg.lidar_net_info.lidar_ipaddr = lidar_ip.GetString();
if (!ParseTypeLidarCfg(object, host_net_info_object[i], device_type, lidar_cfg)) {
return false;
}
custom_lidars_cfg_ptr->push_back(std::move(lidar_cfg));
}
}
return true;
}
bool ParseCfgFile::ParseOldLidarCfg(const rapidjson::Value &object, const uint8_t& device_type, std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr) {
const rapidjson::Value &host_net_info_object = object["host_net_info"];
LivoxLidarCfg lidar_cfg;
if (!ParseTypeLidarCfg(object, host_net_info_object, device_type, lidar_cfg)) {
return false;
}
lidars_cfg_ptr->push_back(std::move(lidar_cfg));
return true;
}
bool ParseCfgFile::ParseTypeLidarCfg(const rapidjson::Value &object, const rapidjson::Value &host_net_info_object, const uint8_t& device_type, LivoxLidarCfg& lidar_cfg) {
lidar_cfg.device_type = device_type;
if (!ParseLidarNetInfo(object, lidar_cfg.lidar_net_info)) {
LOG_ERROR("Parse hap lidar net info failed.");
return false;
}
if (!ParseHostNetInfo(host_net_info_object, lidar_cfg.host_net_info)) {
LOG_ERROR("Parse host net info failed.");
return false;
}
if (!ParseGeneralCfgInfo(object, lidar_cfg.general_cfg_info)) {
LOG_ERROR("Parse general cfg failed");
return false;
}
return true;
}
bool ParseCfgFile::ParseLidarNetInfo(const rapidjson::Value &object, LivoxLidarNetInfo& lidar_net_info) {
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.");
return false;
}
const rapidjson::Value &lidar_net_info_object = object["lidar_net_info"];
if (!lidar_net_info_object.HasMember("cmd_data_port") || !lidar_net_info_object["cmd_data_port"].IsUint()) {
LOG_ERROR("Parse lidar net info failed, has not cmd_data_port member or cmd_data_port is not uint.");
return false;
}
lidar_net_info.cmd_data_port = lidar_net_info_object["cmd_data_port"].GetUint();
if (!lidar_net_info_object.HasMember("push_msg_port") || !lidar_net_info_object["push_msg_port"].IsUint()) {
LOG_ERROR("Parse lidar net info failed, has not push_msg_port member or push_msg_port is not uint.");
return false;
}
lidar_net_info.push_msg_port = lidar_net_info_object["push_msg_port"].GetUint();
if (!lidar_net_info_object.HasMember("point_data_port") || !lidar_net_info_object["point_data_port"].IsUint()) {
LOG_ERROR("Parse lidar net info failed, has not point_data_port member or point_data_port is not uint.");
return false;
}
lidar_net_info.point_data_port = lidar_net_info_object["point_data_port"].GetUint();
if (!lidar_net_info_object.HasMember("imu_data_port") || !lidar_net_info_object["imu_data_port"].IsUint()) {
LOG_ERROR("Parse lidar net info failed, has not imu_data_port member or imu_data_port is not uint.");
return false;
}
lidar_net_info.imu_data_port = lidar_net_info_object["imu_data_port"].GetUint();
if (!lidar_net_info_object.HasMember("log_data_port") || !lidar_net_info_object["log_data_port"].IsUint()) {
LOG_ERROR("Parse lidar net info failed, has not log_data_port member or log_data_port is not uint.");
return false;
}
lidar_net_info.log_data_port = lidar_net_info_object["log_data_port"].GetUint();
return true;
}
bool ParseCfgFile::ParseHostNetInfo(const rapidjson::Value &host_net_info_object, HostNetInfo& host_net_info) {
if (!host_net_info_object.HasMember("host_ip") && !host_net_info_object.HasMember("cmd_data_ip")) {
LOG_ERROR("Parse host net info failed, has not host_ip or cmd_data_ip.");
return false;
}
if (host_net_info_object.HasMember("host_ip") && !host_net_info_object["host_ip"].IsString()) {
LOG_ERROR("Parse host net info failed, host_ip is not string.");
return false;
}
if (host_net_info_object.HasMember("cmd_data_ip") && !host_net_info_object["cmd_data_ip"].IsString()) {
LOG_ERROR("Parse host net info failed, cmd_data_ip is not string.");
return false;
}
// parse cmd ip info
if (host_net_info_object.HasMember("cmd_data_ip") && host_net_info_object["cmd_data_ip"].IsString()) {
host_net_info.host_ip = host_net_info_object["cmd_data_ip"].GetString();
}
// parse host ip info
if (host_net_info_object.HasMember("host_ip") && host_net_info_object["host_ip"].IsString()) {
host_net_info.host_ip = host_net_info_object["host_ip"].GetString();
}
// parse multicast ip info
if (host_net_info_object.HasMember("multicast_ip")) {
if (host_net_info_object["multicast_ip"].IsString()) {
host_net_info.multicast_ip = host_net_info_object["multicast_ip"].GetString();
} else {
LOG_ERROR("Parse host net info failed, has not multicast_ip or multicast_ip is not string.");
return false;
}
} else {
host_net_info.multicast_ip = "";
}
// parse cmd port info
if (!host_net_info_object.HasMember("cmd_data_port") || !host_net_info_object["cmd_data_port"].IsUint()) {
LOG_ERROR("Parse host net info failed, has not cmd_data_port or cmd_data_port is not uint.");
return false;
}
host_net_info.cmd_data_port = host_net_info_object["cmd_data_port"].GetUint();
// parse push msg port
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_port = host_net_info_object["push_msg_port"].GetUint();
// parse point data port
if (!host_net_info_object.HasMember("point_data_port") || !host_net_info_object["point_data_port"].IsUint()) {
LOG_ERROR("Parse host net info failed, has not point_data_port or point_data_port is not uint.");
return false;
}
host_net_info.point_data_port = host_net_info_object["point_data_port"].GetUint();
// parse imu data port
if (!host_net_info_object.HasMember("imu_data_port") || !host_net_info_object["imu_data_port"].IsUint()) {
LOG_ERROR("Parse host net info failed, has not imu_data_port or imu_data_port is not uint.");
return false;
}
host_net_info.imu_data_port = host_net_info_object["imu_data_port"].GetUint();
// parse log port
if (!host_net_info_object.HasMember("log_data_port") || !host_net_info_object["log_data_port"].IsUint()) {
LOG_ERROR("Parse host net info failed, has not cmd_data_port or cmd_data_port is not uint.");
return false;
}
host_net_info.log_data_port = host_net_info_object["log_data_port"].GetUint();
return true;
}
bool ParseCfgFile::ParseGeneralCfgInfo(const rapidjson::Value &object, GeneralCfgInfo& general_cfg_info) {
return true;
}
} // namespace lidar
} // namespace livox
sdk_core/parse_cfg_file.h
+76
View File
@@ -0,0 +1,76 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_PARSE_CFG_FILE_H_
#define LIVOX_PARSE_CFG_FILE_H_
#include "livox_lidar_def.h"
#include "rapidjson/document.h"
#include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
#include "comm/define.h"
#include <iostream>
#include <string>
#include <memory>
#include <vector>
namespace livox {
namespace lidar {
class ParseCfgFile {
public:
explicit ParseCfgFile(const std::string& path);
bool Parse(std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr,
std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr,
std::shared_ptr<LivoxLidarLoggerCfg>& lidar_logger_cfg_ptr,
std::shared_ptr<LivoxLidarSdkFrameworkCfg>& sdk_framework_cfg_ptr
);
private:
bool ParseLidarCfg(const rapidjson::Value &object,
const uint8_t& device_type,
std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr,
std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr);
bool ParseNewLidarCfg(const rapidjson::Value &object,
const uint8_t& device_type,
std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr,
std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr);
bool ParseOldLidarCfg(const rapidjson::Value &object,
const uint8_t& device_type,
std::shared_ptr<std::vector<LivoxLidarCfg>>& lidars_cfg_ptr);
bool ParseTypeLidarCfg(const rapidjson::Value &object, const rapidjson::Value &host_net_info_object, const uint8_t& device_type, LivoxLidarCfg& lidar_cfg);
bool ParseLidarNetInfo(const rapidjson::Value &object, LivoxLidarNetInfo& lidar_net_info);
bool ParseHostNetInfo(const rapidjson::Value &host_net_info_object, HostNetInfo& host_net_info);
bool ParseGeneralCfgInfo(const rapidjson::Value &object, GeneralCfgInfo& general_cfg_info);
private:
const std::string path_;
};
} // namesace lidar
} // namespace livox
#endif // LIVOX_PARSE_CFG_FILE_H_
sdk_core/upgrade/firmware.cpp
+105
View File
@@ -0,0 +1,105 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include <string.h>
#include "firmware.h"
namespace livox {
namespace lidar {
Firmware::Firmware() : data_(nullptr), file_size_(0) {
memset((void *)&header_, 0, sizeof(header_));
memset((void *)&tail_, 0, sizeof(tail_));
}
Firmware::~Firmware() {
if (data_) {
delete[] data_;
}
}
void Firmware::Close() {
file_.close();
}
bool Firmware::Open(const char *firmware_path) {
if (!firmware_path) {
return false;
}
file_.open(firmware_path,
std::ios::in | std::ifstream::binary | std::ios_base::ate);
if (!file_.is_open()) {
printf("Open %s firmware file fail!\n", firmware_path);
return false;
}
file_size_ = file_.tellg();
file_.seekg(0, std::ios::beg);
if (file_size_ < MiniFileSize()) {
printf("Firmware file size is too small!\n");
return false;
}
if (!ReadAndCheckHeader()) {
printf("Firmware file header is wrong!\n");
return false;
}
//printf("Firmware file total size %lu\n", file_size_);
return true;
}
uint64_t Firmware::MiniFileSize() {
return sizeof(header_) + sizeof(tail_) + 1;
}
bool Firmware::ReadAndCheckHeader() {
file_.seekg(0, std::ios::beg);
file_.read((char *)(&header_), sizeof(header_));
printf("This firmware is used for device[%d].\n", header_.device_type);
uint16_t crc = crc16_.mcrf4xx(
(uint8_t *)(&header_), (uint16_t)(sizeof(header_) - sizeof(uint16_t)));
if (crc != header_.header_checksum) {
printf("Header checksum[%4x %4x] error!\n", crc, header_.header_checksum);
return false;
}
printf("Firmware raw data size : %d\n", header_.firmware_length);
data_ = new uint8_t[header_.firmware_length];
file_.read((char *)(data_), header_.firmware_length);
file_.read((char *)(&tail_), sizeof(tail_));
if (file_) {
printf("All firmware data have be read successfully.\n");
} else {
printf("Read firmware fail[%ld]!\n", file_.gcount());
}
return true;
}
}
}
sdk_core/upgrade/firmware.h
+141
View File
@@ -0,0 +1,141 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_UPGRADE_FIRMWARE_H_
#define LIVOX_UPGRADE_FIRMWARE_H_
#include <fstream>
#include <ios>
#include "FastCRC/FastCRC.h"
namespace livox {
namespace lidar {
const uint32_t kMd5SignatureLength = 16;
const uint32_t kEnlFileVersionV2 = 0x02000000;
const uint32_t kEnlFileVersionV3 = 0x03000000;
typedef enum {
kFirmwareMultiApp = 0,
kFirmwareApp = 1,
kFirmwareLoader = 2,
kFirmwareUnknown = 3,
} FirmwareType;
typedef enum {
kFirmwareDeviceTypeHub = 0, /**< Livox Hub. */
kFirmwareDeviceTypeLidarMid40 = 1, /**< Mid-40. */
kFirmwareDeviceTypeLidarTele = 2, /**< Tele. */
kFirmwareDeviceTypeLidarHorizon = 3, /**< Horizon. */
kFirmwareDeviceTypeLidarHubV2 = 4, /**< HubV2. */
kFirmwareDeviceTypeLidarMidLite = 5, /**< Mid-Lite. */
kFirmwareDeviceTypeLidarMid70 = 6, /**< Mid-70. */
kFirmwareDeviceTypeLidarAvia = 7, /**< Avia. */
kFirmwareDeviceTypeLidarXxx1 = 8, /**< xxx1. */
kFirmwareDeviceTypeLidarXxx2 = 9, /**< xxx2. */
kFirmwareDeviceTypeLidarHap = 10, /**< Hap */
kFirmwareDeviceUnkown
} FirmwareDeviceType;
#pragma pack(1)
// typedef struct {
// uint32_t file_version;
// uint32_t firmware_version;
// uint32_t firmware_length;
// uint8_t firmware_type;
// uint8_t device_type;
// uint8_t encrypt_type;
// uint8_t rsvd[2];
// uint8_t checksum_type;
// uint16_t checksum_length;
// uint8_t checksum[256];
// uint64_t modify_time;
// uint16_t header_checksum;
// } LivoxEncryptFirmwareHeader;
typedef struct {
uint32_t file_version;
uint32_t firmware_version;
uint32_t firmware_length;
uint8_t firmware_type;
uint8_t device_type;
uint8_t encrypt_type;
uint8_t rsvd[2];
uint8_t checksum_type;
uint16_t checksum_length;
uint8_t checksum[128];
uint8_t hw_whitelist[128];
uint64_t modify_time;
uint16_t header_checksum;
} LivoxEncryptFirmwareHeader;
typedef struct {
uint8_t overall_signature[kMd5SignatureLength];
} LivoxEncryptFirmwareTail;
/** Lidar feature. */
typedef enum {
kEverythingIsOk = 0,
kFirmwareOutOfLength = 1,
kSystemIsNotReady = 2,
kFirmwareTypeMismatch = 3,
kUpgradeStateMismatch = 4,
} RequestUpgradeReturnCode;
const uint32_t kGeneralTryCountLimit = 10;
const uint32_t kGetProcessTryCountLimit = 30;
const uint32_t kGetProgressTryCountLimit = 10;
#pragma pack()
class Firmware {
public:
Firmware();
~Firmware();
bool Open(const char *firmware_path);
void Close();
const uint32_t FirmwarePackageVersion() const { return header_.file_version; }
LivoxEncryptFirmwareHeader header_;
uint8_t *data_;
LivoxEncryptFirmwareTail tail_;
uint64_t file_size_;
private:
uint64_t MiniFileSize();
bool ReadAndCheckHeader();
std::ifstream file_;
FastCRC16 crc16_;
};
}
}
#endif //LIVOX_UPGRADE_FIRMWARE_H_
sdk_core/upgrade/livox_lidar_upgrader.cpp
+357
View File
@@ -0,0 +1,357 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "livox_lidar_upgrader.h"
#include "../command_handler/command_impl.h"
#include <string.h>
namespace livox {
namespace lidar {
typedef int32_t (LivoxLidarUpgrader::*FnFsmEvent)();
typedef struct {
uint32_t state;
uint32_t event;
FnFsmEvent event_handler;
uint32_t next_state;
} FsmEventTable;
const FsmEventTable upgrade_event_table[] = {
{kLivoxLidarUpgradeIdle, kLivoxLidarEventRequestUpgrade, &LivoxLidarUpgrader::StartUpgrade, kLivoxLidarUpgradeRequest},
{kLivoxLidarUpgradeRequest, kLivoxLidarEventRequestUpgrade, &LivoxLidarUpgrader::StartUpgrade, kLivoxLidarUpgradeRequest},
{kLivoxLidarUpgradeRequest, kLivoxLidarEventXferFirmware, &LivoxLidarUpgrader::XferFirmware, kLivoxLidarUpgradeXferFirmware},
{kLivoxLidarUpgradeXferFirmware, kLivoxLidarEventXferFirmware, &LivoxLidarUpgrader::XferFirmware, kLivoxLidarUpgradeXferFirmware},
{kLivoxLidarUpgradeXferFirmware, kLivoxLidarEventCompleteXferFirmware, &LivoxLidarUpgrader::CompleteXferFirmware, kLivoxLidarUpgradeCompleteXferFirmware},
{kLivoxLidarUpgradeCompleteXferFirmware, kLivoxLidarEventCompleteXferFirmware, &LivoxLidarUpgrader::CompleteXferFirmware, kLivoxLidarUpgradeCompleteXferFirmware},
{kLivoxLidarUpgradeCompleteXferFirmware, kLivoxLidarEventGetUpgradeProgress, &LivoxLidarUpgrader::GetUpgradeProgress, kLivoxLidarUpgradeGetUpgradeProgress},
{kLivoxLidarUpgradeGetUpgradeProgress, kLivoxLidarEventGetUpgradeProgress, &LivoxLidarUpgrader::GetUpgradeProgress, kLivoxLidarUpgradeGetUpgradeProgress},
{kLivoxLidarUpgradeGetUpgradeProgress, kLivoxLidarEventComplete, &LivoxLidarUpgrader::UpgradeComplete, kLivoxLidarUpgradeComplete},
{kLivoxLidarUpgradeComplete, kLivoxLidarEventComplete, &LivoxLidarUpgrader::UpgradeComplete, kLivoxLidarUpgradeComplete},
{kLivoxLidarUpgradeComplete, kLivoxLidarEventReinit, nullptr, kLivoxLidarUpgradeIdle}
// {kUpgradeRebootDevice, kLivoxLidarEventReinit, nullptr, kLivoxLidarUpgradeIdle},
};
LivoxLidarUpgrader::LivoxLidarUpgrader(const Firmware& firmware, const uint32_t handle)
: firmware_(firmware), read_offset_(0), read_length_(1024), handle_(handle), fsm_state_(0),
upgrade_error_(0), progress_(0), try_count_(0) {}
LivoxLidarUpgrader::~LivoxLidarUpgrader() {
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
if (IsUpgradeError()) {
printf("LivoxLidar lidar[%u] upgrade error, try again please!\r\n", handle_);
break;
}
if (IsUpgradeComplete()) {
printf("LivoxLidar lidar[%u] upgrade successfully.\r\n", handle_);
break;
}
}
if (upgrade_thread_ && upgrade_thread_->joinable()) {
upgrade_thread_->join();
upgrade_thread_ = nullptr;
}
}
void LivoxLidarUpgrader::AddUpgradeProgressObserver(UpgradeProgressCallback observer) {
observer_ = observer;
return;
}
bool LivoxLidarUpgrader::StartUpgradeLivoxLidar() {
upgrade_thread_ = std::make_shared<std::thread>([this](){
this->FsmEventHandler(kLivoxLidarEventRequestUpgrade, 10);
});
return true;
}
void LivoxLidarUpgrader::FsmEventHandler(LivoxLidarFsmEvent event, uint8_t progress) {
FnFsmEvent event_handler = nullptr;
if ((event == kLivoxLidarEventTimeout) || (event == kLivoxLidarEventErr)) {
LivoxLidarFsmStateChange(event);
}
printf("Fsm event handler, the livox_lidar[%u] State[%d] | Event[%d]\r\n", handle_, 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 livox_lidar[%u] New State[%d] | Event[%d]\r\n", handle_, fsm_state_, event);
break;
}
}
if (event_handler) {
(this->*event_handler)();
}
if (observer_) {
LivoxLidarUpgradeState upgrade_state = {event, progress};
observer_(handle_, upgrade_state);
}
}
livox_status LivoxLidarUpgrader::StartUpgrade() {
read_offset_ = 0;
upgrade_error_ = 0;
progress_ = 0;
uint8_t request_buf[1024] = { 0 };
LivoxLidarStartUpgradeRequest *request = (LivoxLidarStartUpgradeRequest *)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;
printf("Start upgrade, the livox_lidar[%u] device type [%d]\r\n", handle_, request->dev_type);
if (kEnlFileVersionV3 == firmware_.FirmwarePackageVersion()) {
LivoxLidarStartUpgradeRequestV3 *request_v3 = (LivoxLidarStartUpgradeRequestV3 *)request_buf;
request_v3->firmware_version = firmware_.header_.firmware_version;
request_v3->firmware_buildtime = firmware_.header_.modify_time;
memcpy(request_v3->hw_whitelist, firmware_.header_.hw_whitelist, sizeof(request_v3->hw_whitelist));
return CommandImpl::LivoxLidarStartUpgrade(handle_, request_buf, sizeof(LivoxLidarStartUpgradeRequestV3), StartUpgradeResponseHandler, this);
} else {
return CommandImpl::LivoxLidarStartUpgrade(handle_, request_buf, sizeof(LivoxLidarStartUpgradeRequest), StartUpgradeResponseHandler, this);
}
}
livox_status LivoxLidarUpgrader::XferFirmware() {
uint8_t send_bufer[2048];
LivoxLidarXferFirmwareResquest* request = (LivoxLidarXferFirmwareResquest*)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 livox_lidar[%u] xfer firmware failed, Read_offset is err, firmware_length[%d], read_offset[%d].\r\n",
handle_, 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;
std::this_thread::sleep_for(std::chrono::milliseconds(5));
printf("The livox_lidar[%u] xfer firmware read offset %d\r\n", handle_, request->offset);
return CommandImpl::LivoxLidarXferFirmware(handle_, (uint8_t *)request,
sizeof(LivoxLidarXferFirmwareResquest) + read_length - sizeof(request->data),
XferFirmwareResponseHandler, this);
}
livox_status LivoxLidarUpgrader::CompleteXferFirmware() {
uint8_t send_bufer[2048];
LivoxLidarCompleteXferFirmwareResquest* request = (LivoxLidarCompleteXferFirmwareResquest*)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 CommandImpl::LivoxLidarCompleteXferFirmware(handle_, (uint8_t *)request,
sizeof(LivoxLidarCompleteXferFirmwareResquest) +
request->checksum_length - sizeof(request->checksum),
CompleteXferFirmwareResponseHandler, this);
}
livox_status LivoxLidarUpgrader::GetUpgradeProgress() {
return CommandImpl::LivoxLidarGetUpgradeProgress(handle_, nullptr, 0, GetProgressResponseHandler, this);
}
livox_status LivoxLidarUpgrader::UpgradeComplete() {
return CommandImpl::LivoxLidarRequestReboot(handle_, UpgradeCompleteResponseHandler, this);
}
int32_t LivoxLidarUpgrader::LivoxLidarFsmStateChange(LivoxLidarFsmEvent event) {
if (event < kLivoxLidarEventUndef) {
fsm_state_ = event;
}
return 0;
}
/**
* Upgrade Callback handler
*/
void LivoxLidarUpgrader::StartUpgradeResponseHandler(livox_status status, uint32_t handle,
LivoxLidarStartUpgradeResponse* response, void* client_data) {
LivoxLidarUpgrader* upgrade = static_cast<LivoxLidarUpgrader *>(client_data);
if (status == kLivoxLidarStatusSuccess) {
upgrade->try_count_ = 0;
printf("Start upgrade the livox_lidar[%u]\r\n", handle);
if (response->ret_code) {
if (response->ret_code == kSystemIsNotReady) {
printf("Start upgrade failed, the livox_lidar[%u] is busy, try again!\r\n", handle);
upgrade->FsmEventHandler(kLivoxLidarEventRequestUpgrade, 10);
} else if (response->ret_code == EraseFirmware) {
std::this_thread::sleep_for(std::chrono::seconds(1));
printf("Start upgrade, erase livox_lidar[%u] firmware!\r\n", handle);
upgrade->FsmEventHandler(kLivoxLidarEventRequestUpgrade, 10);
} else {
printf("Start upgrade failed, the livox_lidar[%u] ret_code[%d], try again!\r\n", handle, response->ret_code);
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
}
} else {
printf("Start upgrade succ, the livox_lidar[%u] start to xfer data!\r\n", handle);
upgrade->FsmEventHandler(kLivoxLidarEventXferFirmware, 20);
}
} else {
printf("Start upgrade failed, the livox_lidar[%u] status:%d start upgrade is timeout[%d], try again!\r\n", handle, status, upgrade->try_count_);
++upgrade->try_count_;
if (upgrade->try_count_ < kGeneralTryCountLimit) {
upgrade->FsmEventHandler(kLivoxLidarEventRequestUpgrade, 10);
} else {
upgrade->try_count_ = 0;
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
printf("Start upgrade failed, the livox_lidar[%u] start upgrade exceed limit! exit!\r\n", handle);
}
}
}
void LivoxLidarUpgrader::XferFirmwareResponseHandler(livox_status status, uint32_t handle,
LivoxLidarXferFirmwareResponse* response, void* client_data) {
LivoxLidarUpgrader* upgrade = static_cast<LivoxLidarUpgrader *>(client_data);
if (status == kLivoxLidarStatusSuccess) {
upgrade->try_count_ = 0;
if (response->ret_code) {
printf("The livox_lidar[%u] Xfer firmware fail[%d]\r\n", handle, response->ret_code);
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
} else {
upgrade->read_offset_ += upgrade->read_length_;
if (upgrade->read_offset_ < upgrade->firmware_.header_.firmware_length) {
upgrade->FsmEventHandler(kLivoxLidarEventXferFirmware, 20);
} else {
printf("Xfer firmware succ, the livox_lidar[%u] last offset[%d]\r\n", handle, upgrade->read_offset_);
upgrade->FsmEventHandler(kLivoxLidarEventCompleteXferFirmware, 40);
}
}
} else {
printf("Xfer firmware failed, the livox_lidar[%u] xfer firmware timeout, try_count:%d, try again!\r\n", handle, upgrade->try_count_);
++upgrade->try_count_;
if (upgrade->try_count_ < kGeneralTryCountLimit) {
upgrade->FsmEventHandler(kLivoxLidarEventXferFirmware, 20);
} else {
upgrade->try_count_ = 0;
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
printf("Xfer firmware failed, the livox_lidar[%u] exceed limit! exit!\r\n", handle);
}
}
}
void LivoxLidarUpgrader::CompleteXferFirmwareResponseHandler(livox_status status, uint32_t handle,
LivoxLidarCompleteXferFirmwareResponse* response, void* client_data) {
LivoxLidarUpgrader* upgrade = static_cast<LivoxLidarUpgrader *>(client_data);
if (status == kLivoxLidarStatusSuccess) {
upgrade->try_count_ = 0;
if (response->ret_code) {
printf("Complete xfer failed, the livox_lidar[%u] ret_code:%d.\r\n", handle, response->ret_code);
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
} else {
printf("The livox_lidar[%u] complete xfer succ.\n", handle);
upgrade->FsmEventHandler(kLivoxLidarEventGetUpgradeProgress, 50);
}
} else {
printf("Complete xfer failed, the livox_lidar[%u] complete xfer is timeout, try_count:%d, try again!\r\n", handle, upgrade->try_count_);
++upgrade->try_count_;
if (upgrade->try_count_ < kGeneralTryCountLimit) {
upgrade->FsmEventHandler(kLivoxLidarEventCompleteXferFirmware, 50);
} else {
upgrade->try_count_ = 0;
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
printf("Complete xfer failed, the livox_lidar[%u] complete xfer exceed limit! exit!\r\n", handle);
}
}
}
void LivoxLidarUpgrader::GetProgressResponseHandler(livox_status status, uint32_t handle,
LivoxLidarGetUpgradeProgressResponse* response, void* client_data) {
LivoxLidarUpgrader* upgrade = static_cast<LivoxLidarUpgrader *>(client_data);
if (status == kLivoxLidarStatusSuccess) {
upgrade->try_count_ = 0;
if (response->ret_code) {
printf("Get progress failed, the livox_lidar[%u] ret_code:%d.\r\n", handle, response->ret_code);
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
} else {
if (response->progress < 100) {
printf("The livox_lidar[%u] get progress[%u]\r\n", handle, response->progress);
upgrade->FsmEventHandler(kLivoxLidarEventGetUpgradeProgress, response->progress/2 + 50);
} else {
printf("The livox_lidar[%u] Get progress[%u]\r\n", handle, response->progress);
upgrade->FsmEventHandler(kLivoxLidarEventComplete, 100);
}
}
} else {
//printf("Get progress failed, the livox_lidar[%d] get progress timeout, try_count:%d, try again!\r\n", handle, upgrade->try_count_);
++upgrade->try_count_;
if (upgrade->try_count_ < kGetProcessTryCountLimit) {
upgrade->FsmEventHandler(kLivoxLidarEventGetUpgradeProgress, upgrade->progress_/2 + 50);
} else {
upgrade->try_count_ = 0;
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
printf("Get progress failed, the livox_lidar[%u] get progress exceed limit! exit!\r\n", handle);
}
}
}
void LivoxLidarUpgrader::UpgradeCompleteResponseHandler(livox_status status, uint32_t handle,
LivoxLidarRebootResponse* response, void* client_data) {
LivoxLidarUpgrader* upgrade = static_cast<LivoxLidarUpgrader *>(client_data);
if (status == kLivoxLidarStatusSuccess) {
upgrade->try_count_ = 0;
if (response->ret_code) {
printf("Upgrade complete failed, the livox_lidar[%u], ret_code[%d] reboot device fail!\r\n", handle, response->ret_code);
upgrade->FsmEventHandler(kLivoxLidarEventErr, 100);
} else {
printf("The livox_lidar[%u] upgrade complete succ.\n", handle);
upgrade->FsmEventHandler(kLivoxLidarEventReinit, 100);
}
} else {
printf("Upgrade complete failed, the livox_lidar[%u] reboot device timeout, try_count:%d, try again!\r\n",
handle, upgrade->try_count_);
++upgrade->try_count_;
if (upgrade->try_count_ < kGeneralTryCountLimit) {
upgrade->FsmEventHandler(kLivoxLidarEventComplete, 100);
} else {
upgrade->try_count_ = 0;
upgrade->FsmEventHandler(kLivoxLidarEventReinit, 100);
printf("Upgrade complete failed, the livox_lidar[%u] reboot device exceed limit! exit!\r\n", handle);
}
}
}
} // namespace comm
} // namespace livox
sdk_core/upgrade/livox_lidar_upgrader.h
+95
View File
@@ -0,0 +1,95 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_LIDAR_UPGRADER_H_
#define LIVOX_LIDAR_UPGRADER_H_
#include <fstream>
#include <ios>
#include <memory>
#include <thread>
#include <functional>
#include "firmware.h"
#include "../comm/define.h"
#include "livox_lidar_def.h"
namespace livox {
namespace lidar {
static const uint8_t EraseFirmware = 0x34;
class LivoxLidarUpgrader {
public:
using UpgradeProgressCallback = std::function<void(uint32_t handle, LivoxLidarUpgradeState state)>;
LivoxLidarUpgrader(const Firmware& firmware, const uint32_t handle);
~LivoxLidarUpgrader();
void AddUpgradeProgressObserver(UpgradeProgressCallback observer);
bool StartUpgradeLivoxLidar();
livox_status StartUpgrade();
livox_status XferFirmware();
livox_status CompleteXferFirmware();
livox_status GetUpgradeProgress();
livox_status UpgradeComplete();
static void StartUpgradeResponseHandler(livox_status status, uint32_t handle,
LivoxLidarStartUpgradeResponse* response, void* client_data);
static void XferFirmwareResponseHandler(livox_status status, uint32_t handle,
LivoxLidarXferFirmwareResponse* response, void* client_data);
static void CompleteXferFirmwareResponseHandler(livox_status status, uint32_t handle,
LivoxLidarCompleteXferFirmwareResponse* response, void* client_data);
static void GetProgressResponseHandler(livox_status status, uint32_t handle,
LivoxLidarGetUpgradeProgressResponse* response, void* client_data);
static void UpgradeCompleteResponseHandler(livox_status status, uint32_t handle,
LivoxLidarRebootResponse* response, void* client_data);
int32_t LivoxLidarFsmStateChange(LivoxLidarFsmEvent event);
void FsmEventHandler(LivoxLidarFsmEvent event, uint8_t progress);
bool IsUpgradeComplete() { return (fsm_state_ == kLivoxLidarUpgradeIdle); }
bool IsUpgradeError() { return (fsm_state_ == kLivoxLidarUpgradeTimeout) || (fsm_state_ == kLivoxLidarUpgradeErr); }
private:
const Firmware& firmware_;
uint32_t read_offset_;
uint32_t read_length_;
uint32_t handle_;
uint32_t fsm_state_;
uint32_t upgrade_error_;
uint8_t progress_;
uint32_t try_count_;
std::shared_ptr<std::thread> upgrade_thread_;
UpgradeProgressCallback observer_;
};
} // namespace comm
} // namespace LIVOX_LIDAR_UPGRADER_H_
#endif
sdk_core/upgrade_manager.cpp
+84
View File
@@ -0,0 +1,84 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#include "upgrade_manager.h"
#include <thread>
namespace livox {
namespace lidar {
UpgradeManager& UpgradeManager::GetInstance() {
static UpgradeManager manager;
return manager;
}
UpgradeManager::UpgradeManager()
: livox_lidar_info_cb_(nullptr),
livox_lidar_client_data_(nullptr) {
}
bool UpgradeManager::SetLivoxLidarUpgradeFirmwarePath(const char* firmware_path) {
if (!livox_lidar_firmware_.Open(firmware_path)) {
printf("Open firmware_path fail\r\n");
return false;
}
return true;
}
void UpgradeManager::SetLivoxLidarUpgradeProgressCallback(OnLivoxLidarUpgradeProgressCallback cb, void* client_data) {
livox_lidar_info_cb_ = cb;
livox_lidar_client_data_ = client_data;
}
void UpgradeManager::UpgradeLivoxLidars(const uint32_t* handle, const uint8_t lidar_num) {
std::vector<LivoxLidarUpgrader> upgrader_vec;
upgrader_vec.reserve(lidar_num);
for (size_t i = 0; i < lidar_num; ++i) {
LivoxLidarUpgrader upgrader(livox_lidar_firmware_, handle[i]);
OnLivoxLidarUpgradeProgressCallback cb = livox_lidar_info_cb_;
void* client_data = livox_lidar_client_data_;
upgrader.AddUpgradeProgressObserver([cb, client_data](uint32_t handle, LivoxLidarUpgradeState state) {
if (cb) {
cb(handle, state, client_data);
}
});
upgrader_vec.emplace_back(std::move(upgrader));
}
for (size_t i = 0; i < upgrader_vec.size(); ++i) {
LivoxLidarUpgrader& upgrader = upgrader_vec[i];
upgrader.StartUpgradeLivoxLidar();
}
CloseLivoxLidarFirmwareFile();
}
void UpgradeManager::CloseLivoxLidarFirmwareFile() {
livox_lidar_firmware_.Close();
}
} // namespace comm
} // namespace livox
sdk_core/upgrade_manager.h
+71
View File
@@ -0,0 +1,71 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2022 Livox. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
#ifndef LIVOX_UPGRADE_MANAGER_H_
#define LIVOX_UPGRADE_MANAGER_H_
#include <functional>
#include <memory>
#include <fstream>
#include <vector>
#include <string>
#include "upgrade/livox_lidar_upgrader.h"
#include "upgrade/firmware.h"
namespace livox {
namespace lidar {
class UpgradeManager {
public:
using OnLivoxLidarUpgradeProgressCallback =
std::function<void(uint32_t handle, LivoxLidarUpgradeState state, void *client_data)>;
private:
UpgradeManager();
UpgradeManager(const UpgradeManager& other) = delete;
UpgradeManager& operator=(const UpgradeManager& other) = delete;
public:
typedef std::chrono::steady_clock::time_point TimePoint;
void Destory();
~UpgradeManager() = default;
static UpgradeManager& GetInstance();
// Livox lidar upgrade
bool SetLivoxLidarUpgradeFirmwarePath(const char* firmware_path);
void SetLivoxLidarUpgradeProgressCallback(OnLivoxLidarUpgradeProgressCallback cb, void* client_data);
void UpgradeLivoxLidars(const uint32_t* handle, const uint8_t lidar_num);
void CloseLivoxLidarFirmwareFile();
private:
Firmware livox_lidar_firmware_;
OnLivoxLidarUpgradeProgressCallback livox_lidar_info_cb_;
void *livox_lidar_client_data_;
};
UpgradeManager &upgrade_manager();
} // namespace comm
} // namespace livox
#endif // LIVOX_UPGRADE_MANAGER_H_