Merge commit '457f054053fbd5277ca31ab40275a1d0802bbbc0' as 'lidar/sllidar_ros2'
This commit is contained in:
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
* External Reference and dependencies
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "sdkcommon.h"
|
||||
#include "hal/abs_rxtx.h"
|
||||
#include "hal/thread.h"
|
||||
#include "hal/types.h"
|
||||
#include "hal/assert.h"
|
||||
#include "hal/locker.h"
|
||||
#include "hal/socket.h"
|
||||
#include "hal/event.h"
|
||||
#include "hal/waiter.h"
|
||||
#include "hal/byteorder.h"
|
||||
#include "sl_lidar_driver.h"
|
||||
#include "sl_crc.h"
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
|
||||
#define CONF_NO_BOOST_CRC_SUPPORT
|
||||
|
||||
#include "dataupacker_namespace.h"
|
||||
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
* Internal Definition
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
BEGIN_DATAUNPACKER_NS()
|
||||
|
||||
|
||||
class LIDARSampleDataUnpackerInner: public LIDARSampleDataUnpacker
|
||||
{
|
||||
public:
|
||||
LIDARSampleDataUnpackerInner(LIDARSampleDataListener& l): LIDARSampleDataUnpacker(l){}
|
||||
|
||||
virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0;
|
||||
virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size) = 0;
|
||||
virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size) = 0;
|
||||
virtual void publishNewScanReset() = 0;
|
||||
|
||||
|
||||
virtual _u64 getCurrentTimestamp_uS() = 0;
|
||||
|
||||
};
|
||||
|
||||
class IDataUnpackerHandler
|
||||
{
|
||||
public:
|
||||
IDataUnpackerHandler() {}
|
||||
virtual ~IDataUnpackerHandler() {}
|
||||
|
||||
|
||||
virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) = 0;
|
||||
|
||||
virtual _u8 getSampleAnswerType() const = 0;
|
||||
virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size) = 0;
|
||||
virtual void reset() = 0;
|
||||
|
||||
};
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -0,0 +1,259 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "dataunnpacker_commondef.h"
|
||||
#include "dataunpacker.h"
|
||||
#include "dataunnpacker_internal.h"
|
||||
|
||||
|
||||
#include <map>
|
||||
|
||||
|
||||
#define REGISTER_HANDLER(_c_) { \
|
||||
auto newBorn = new unpacker::_c_(); \
|
||||
if (!newBorn) return false; \
|
||||
handlerList.push_back(newBorn); \
|
||||
}
|
||||
|
||||
// How to include new handlers?
|
||||
// 1. add extra include line below if a new handle is to be included
|
||||
// 2. update the code in function _registerDataUnpackerHandlers
|
||||
#include "unpacker/handler_capsules.h"
|
||||
#include "unpacker/handler_hqnode.h"
|
||||
#include "unpacker/handler_normalnode.h"
|
||||
|
||||
|
||||
#define DEF_REGISTER_HANDLER_LIST
|
||||
|
||||
|
||||
BEGIN_DATAUNPACKER_NS()
|
||||
|
||||
|
||||
static bool _registerDataUnpackerHandlers(std::vector<IDataUnpackerHandler *> & handlerList)
|
||||
{
|
||||
REGISTER_HANDLER(UnpackerHandler_NormalNode);
|
||||
REGISTER_HANDLER(UnpackerHandler_HQNode);
|
||||
REGISTER_HANDLER(UnpackerHandler_CapsuleNode);
|
||||
REGISTER_HANDLER(UnpackerHandler_UltraCapsuleNode);
|
||||
REGISTER_HANDLER(UnpackerHandler_DenseCapsuleNode);
|
||||
REGISTER_HANDLER(UnpackerHandler_UltraDenseCapsuleNode);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
class LIDARSampleDataUnpackerImpl : public LIDARSampleDataUnpackerInner
|
||||
{
|
||||
public:
|
||||
|
||||
void registerHandler(_u8 ansType, IDataUnpackerHandler* handler)
|
||||
{
|
||||
_handlerMap[ansType] = handler;
|
||||
}
|
||||
|
||||
|
||||
void unregisterAllHandlers()
|
||||
{
|
||||
for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr)
|
||||
{
|
||||
delete itr->second;
|
||||
}
|
||||
_handlerMap.clear();
|
||||
}
|
||||
|
||||
LIDARSampleDataUnpackerImpl(LIDARSampleDataListener& l)
|
||||
: LIDARSampleDataUnpackerInner(l)
|
||||
, _enabled(false)
|
||||
, _lastActiveAnsType(0)
|
||||
, _lastActiveHandler(nullptr)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
virtual ~LIDARSampleDataUnpackerImpl()
|
||||
{
|
||||
unregisterAllHandlers();
|
||||
}
|
||||
|
||||
|
||||
virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size)
|
||||
{
|
||||
|
||||
// notify the handlers ...
|
||||
for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr)
|
||||
{
|
||||
itr->second->onUnpackerContextSet(type, data, size);
|
||||
}
|
||||
}
|
||||
|
||||
virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) {
|
||||
if (!_enabled) return false;
|
||||
|
||||
|
||||
if (_lastActiveAnsType != ansType) {
|
||||
onDeselectHandler();
|
||||
|
||||
auto itr = _handlerMap.find(ansType);
|
||||
if (itr != _handlerMap.end()) {
|
||||
onSelectHandler(ansType, itr->second);
|
||||
}
|
||||
else {
|
||||
onSelectHandler(ansType, nullptr);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (_lastActiveHandler) {
|
||||
_lastActiveHandler->onData(this, reinterpret_cast<const _u8 *>(buffer), size);
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void reset()
|
||||
{
|
||||
clearCache();
|
||||
_lastActiveHandler = nullptr;
|
||||
_lastActiveAnsType = 0;
|
||||
|
||||
}
|
||||
|
||||
virtual void enable()
|
||||
{
|
||||
_enabled = true;
|
||||
reset();
|
||||
}
|
||||
|
||||
virtual void disable()
|
||||
{
|
||||
_enabled = false;
|
||||
reset();
|
||||
|
||||
}
|
||||
|
||||
virtual void clearCache()
|
||||
{
|
||||
if (_lastActiveHandler) {
|
||||
_lastActiveHandler->reset();
|
||||
}
|
||||
}
|
||||
|
||||
virtual _u64 getCurrentTimestamp_uS() {
|
||||
return getus();
|
||||
}
|
||||
|
||||
virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node)
|
||||
{
|
||||
_listener.onHQNodeDecoded(timestamp_uS, node);
|
||||
}
|
||||
|
||||
|
||||
virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size)
|
||||
{
|
||||
_listener.onDecodingError(errorType, ansType, payload, size);
|
||||
|
||||
}
|
||||
|
||||
virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size)
|
||||
{
|
||||
_listener.onCustomSampleDataDecoded(ansType, customCode, payload, size);
|
||||
}
|
||||
|
||||
|
||||
virtual void publishNewScanReset()
|
||||
{
|
||||
_listener.onHQNodeScanResetReq();
|
||||
}
|
||||
protected:
|
||||
|
||||
void onSelectHandler(_u8 ansType, IDataUnpackerHandler* handler)
|
||||
{
|
||||
_lastActiveHandler = handler;
|
||||
_lastActiveAnsType = ansType;
|
||||
}
|
||||
|
||||
void onDeselectHandler()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
|
||||
protected:
|
||||
bool _enabled;
|
||||
std::map<_u8, IDataUnpackerHandler*> _handlerMap;
|
||||
|
||||
_u8 _lastActiveAnsType;
|
||||
IDataUnpackerHandler* _lastActiveHandler;
|
||||
};
|
||||
|
||||
LIDARSampleDataUnpacker* LIDARSampleDataUnpacker::CreateInstance(LIDARSampleDataListener& listener)
|
||||
{
|
||||
LIDARSampleDataUnpackerImpl* impl = new LIDARSampleDataUnpackerImpl(listener);
|
||||
|
||||
std::vector<IDataUnpackerHandler*> list;
|
||||
if (!_registerDataUnpackerHandlers(list)) {
|
||||
delete impl;
|
||||
for (auto itr = list.begin(); itr != list.end(); ++itr) {
|
||||
delete* itr;
|
||||
}
|
||||
impl = nullptr;
|
||||
}
|
||||
|
||||
for (auto itr = list.begin(); itr != list.end(); ++itr) {
|
||||
impl->registerHandler((*itr)->getSampleAnswerType(), (*itr));
|
||||
}
|
||||
return impl;
|
||||
}
|
||||
|
||||
void LIDARSampleDataUnpacker::ReleaseInstance(LIDARSampleDataUnpacker* unpacker) {
|
||||
delete unpacker;
|
||||
}
|
||||
|
||||
LIDARSampleDataUnpacker::~LIDARSampleDataUnpacker() {
|
||||
|
||||
}
|
||||
|
||||
LIDARSampleDataUnpacker::LIDARSampleDataUnpacker(LIDARSampleDataListener& l)
|
||||
: _listener(l)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "dataupacker_namespace.h"
|
||||
|
||||
BEGIN_DATAUNPACKER_NS()
|
||||
|
||||
|
||||
class LIDARSampleDataListener
|
||||
{
|
||||
|
||||
|
||||
public:
|
||||
virtual void onHQNodeScanResetReq() = 0;
|
||||
virtual void onHQNodeDecoded(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0;
|
||||
virtual void onCustomSampleDataDecoded(_u8 ansType, _u32 customCode, const void* data, size_t size) {}
|
||||
|
||||
virtual void onDecodingError(int errMsg, _u8 ansType, const void* payload, size_t size) {}
|
||||
};
|
||||
|
||||
class LIDARSampleDataUnpacker
|
||||
{
|
||||
public:
|
||||
enum {
|
||||
ERR_EVENT_ON_EXP_ENCODER_RESET = 0x8001,
|
||||
ERR_EVENT_ON_EXP_CHECKSUM_ERR = 0x8002,
|
||||
};
|
||||
|
||||
enum UnpackerContextType {
|
||||
UNPACKER_CONTEXT_TYPE_LIDAR_UNKNOWN = 0,
|
||||
UNPACKER_CONTEXT_TYPE_LIDAR_TIMING = 1,
|
||||
UNPACKER_CONTEXT_TYPE_TRIANGULATION_OPTICAL_FACTOR = 2,
|
||||
|
||||
};
|
||||
|
||||
virtual ~LIDARSampleDataUnpacker();
|
||||
static LIDARSampleDataUnpacker* CreateInstance(LIDARSampleDataListener& listener);
|
||||
static void ReleaseInstance(LIDARSampleDataUnpacker*);
|
||||
|
||||
virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size) = 0;
|
||||
|
||||
virtual void enable() = 0;
|
||||
virtual void disable() = 0;
|
||||
|
||||
virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) = 0;
|
||||
virtual void reset() = 0;
|
||||
virtual void clearCache() = 0;
|
||||
|
||||
protected:
|
||||
LIDARSampleDataUnpacker(LIDARSampleDataListener&);
|
||||
LIDARSampleDataListener& _listener;
|
||||
|
||||
};
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -0,0 +1,5 @@
|
||||
#pragma once
|
||||
|
||||
|
||||
#define BEGIN_DATAUNPACKER_NS() namespace sl{ namespace internal{
|
||||
#define END_DATAUNPACKER_NS() }}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
* Capsule Style Sample Node Handlers
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
BEGIN_DATAUNPACKER_NS()
|
||||
|
||||
namespace unpacker {
|
||||
|
||||
class UnpackerHandler_CapsuleNode : public IDataUnpackerHandler {
|
||||
public:
|
||||
UnpackerHandler_CapsuleNode();
|
||||
virtual ~UnpackerHandler_CapsuleNode();
|
||||
|
||||
virtual _u8 getSampleAnswerType() const;
|
||||
virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
|
||||
virtual void reset();
|
||||
virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
|
||||
protected:
|
||||
|
||||
void _onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t &, LIDARSampleDataUnpackerInner* engine);
|
||||
|
||||
std::vector<_u8> _cached_scan_node_buf;
|
||||
int _cached_scan_node_buf_pos;
|
||||
bool _is_previous_capsuledataRdy;
|
||||
|
||||
rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata;
|
||||
_u64 _cached_last_data_timestamp_us;
|
||||
|
||||
SlamtecLidarTimingDesc _cachedTimingDesc;
|
||||
};
|
||||
|
||||
class UnpackerHandler_UltraCapsuleNode : public IDataUnpackerHandler {
|
||||
public:
|
||||
UnpackerHandler_UltraCapsuleNode();
|
||||
virtual ~UnpackerHandler_UltraCapsuleNode();
|
||||
|
||||
virtual _u8 getSampleAnswerType() const;
|
||||
virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
|
||||
virtual void reset();
|
||||
virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
|
||||
protected:
|
||||
void _onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
|
||||
|
||||
|
||||
std::vector<_u8> _cached_scan_node_buf;
|
||||
int _cached_scan_node_buf_pos;
|
||||
bool _is_previous_capsuledataRdy;
|
||||
|
||||
rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
|
||||
_u64 _cached_last_data_timestamp_us;
|
||||
|
||||
SlamtecLidarTimingDesc _cachedTimingDesc;
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class UnpackerHandler_DenseCapsuleNode : public IDataUnpackerHandler {
|
||||
public:
|
||||
UnpackerHandler_DenseCapsuleNode();
|
||||
virtual ~UnpackerHandler_DenseCapsuleNode();
|
||||
|
||||
virtual _u8 getSampleAnswerType() const;
|
||||
virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
|
||||
virtual void reset();
|
||||
virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
|
||||
protected:
|
||||
void _onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
|
||||
|
||||
|
||||
std::vector<_u8> _cached_scan_node_buf;
|
||||
int _cached_scan_node_buf_pos;
|
||||
bool _is_previous_capsuledataRdy;
|
||||
|
||||
rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata;
|
||||
_u64 _cached_last_data_timestamp_us;
|
||||
|
||||
SlamtecLidarTimingDesc _cachedTimingDesc;
|
||||
|
||||
};
|
||||
|
||||
|
||||
class UnpackerHandler_UltraDenseCapsuleNode : public IDataUnpackerHandler {
|
||||
public:
|
||||
UnpackerHandler_UltraDenseCapsuleNode();
|
||||
virtual ~UnpackerHandler_UltraDenseCapsuleNode();
|
||||
|
||||
virtual _u8 getSampleAnswerType() const;
|
||||
virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
|
||||
virtual void reset();
|
||||
virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
|
||||
protected:
|
||||
void _onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
|
||||
|
||||
std::vector<_u8> _cached_scan_node_buf;
|
||||
int _cached_scan_node_buf_pos;
|
||||
bool _is_previous_capsuledataRdy;
|
||||
|
||||
rplidar_response_ultra_dense_capsule_measurement_nodes_t _cached_previous_ultra_dense_capsuledata;
|
||||
_u64 _cached_last_data_timestamp_us;
|
||||
|
||||
|
||||
|
||||
int _last_node_sync_bit;
|
||||
int _last_dist_q2;
|
||||
|
||||
SlamtecLidarTimingDesc _cachedTimingDesc;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -0,0 +1,192 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
* HQNode Sample Node Handler
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "../dataunnpacker_commondef.h"
|
||||
#include "../dataunpacker.h"
|
||||
#include "../dataunnpacker_internal.h"
|
||||
|
||||
#ifdef CONF_NO_BOOST_CRC_SUPPORT
|
||||
#include "sl_crc.h"
|
||||
#endif
|
||||
|
||||
#include "handler_hqnode.h"
|
||||
|
||||
BEGIN_DATAUNPACKER_NS()
|
||||
|
||||
namespace unpacker{
|
||||
|
||||
|
||||
static _u64 _getSampleDelayOffsetInHQMode(const SlamtecLidarTimingDesc& timing)
|
||||
{
|
||||
// FIXME: to eval
|
||||
//
|
||||
// guess channel baudrate by LIDAR model ....
|
||||
const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:1000000;
|
||||
|
||||
_u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_hq_t) * 10 / channelBaudRate;
|
||||
|
||||
if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
|
||||
{
|
||||
tranmissionDelay = 100; //dummy value
|
||||
}
|
||||
|
||||
// center of the sample duration
|
||||
const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
|
||||
const _u64 sampleFilterDelay = timing.sample_duration_uS;
|
||||
|
||||
return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS;
|
||||
}
|
||||
|
||||
UnpackerHandler_HQNode::UnpackerHandler_HQNode()
|
||||
: _cached_scan_node_buf_pos(0)
|
||||
{
|
||||
_cached_scan_node_buf.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t));
|
||||
memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
|
||||
}
|
||||
|
||||
UnpackerHandler_HQNode::~UnpackerHandler_HQNode()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
_u8 UnpackerHandler_HQNode::getSampleAnswerType() const
|
||||
{
|
||||
return RPLIDAR_ANS_TYPE_MEASUREMENT_HQ;
|
||||
}
|
||||
|
||||
void UnpackerHandler_HQNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
|
||||
{
|
||||
|
||||
for (size_t pos = 0; pos < cnt; ++pos)
|
||||
{
|
||||
_u8 current_data = data[pos];
|
||||
|
||||
switch (_cached_scan_node_buf_pos)
|
||||
{
|
||||
case 0: // expect the sync byte
|
||||
{
|
||||
if (current_data == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC) {
|
||||
// pass
|
||||
}
|
||||
else {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4: // get bytes to calculate crc ready
|
||||
{
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: // new data ready
|
||||
{
|
||||
_cached_scan_node_buf[sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1] = current_data;
|
||||
_cached_scan_node_buf_pos = 0;
|
||||
rplidar_response_hq_capsule_measurement_nodes_t* nodesData = reinterpret_cast<rplidar_response_hq_capsule_measurement_nodes_t*>(&_cached_scan_node_buf[0]);
|
||||
|
||||
#ifdef CONF_NO_BOOST_CRC_SUPPORT
|
||||
_u32 crcCalc = crc32::getResult(&_cached_scan_node_buf[0], sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 4);
|
||||
|
||||
|
||||
#else
|
||||
// calculate crc with boost crc method
|
||||
boost::crc_optimal<32, 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true> mycrc;
|
||||
std::vector<_u8> crcInputData;
|
||||
crcInputData.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4);
|
||||
memcpy(&crcInputData[0], nodesData, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4);
|
||||
//supplement crcInputData to mutiples of 4
|
||||
int leftBytes = 4 - (crcInputData.size() & 3);
|
||||
for (int i = 0; i < leftBytes; i++)
|
||||
crcInputData.push_back(0);
|
||||
mycrc.process_bytes(&crcInputData[0], crcInputData.size());
|
||||
_u32 crcCalc = mycrc.checksum();
|
||||
|
||||
#endif
|
||||
|
||||
_u32 recvCRC = nodesData->crc32;
|
||||
#ifdef _CPU_ENDIAN_BIG
|
||||
recvCRC = le32_to_cpu(recvCRC);
|
||||
nodesData->time_stamp = le64_to_cpu(nodesData->time_stamp);
|
||||
#endif
|
||||
if (recvCRC == crcCalc)
|
||||
{
|
||||
for (size_t pos = 0; pos < _countof(nodesData->node_hq); ++pos)
|
||||
{
|
||||
rplidar_response_measurement_node_hq_t hqNode = nodesData->node_hq[pos];
|
||||
#ifdef _CPU_ENDIAN_BIG
|
||||
hqNode.angle_z_q14 = le16_to_cpu(hqNode.angle_z_q14);
|
||||
hqNode.dist_mm_q2 = le32_to_cpu(hqNode.dist_mm_q2);
|
||||
#endif
|
||||
engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInHQMode(_cachedTimingDesc), &hqNode);
|
||||
}
|
||||
}
|
||||
else //crc check not passed
|
||||
{
|
||||
engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR
|
||||
, RPLIDAR_ANS_TYPE_MEASUREMENT_HQ, nodesData, sizeof(*nodesData));
|
||||
}
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
}
|
||||
_cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void UnpackerHandler_HQNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
|
||||
{
|
||||
if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
|
||||
assert(size == sizeof(_cachedTimingDesc));
|
||||
_cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
|
||||
}
|
||||
}
|
||||
|
||||
void UnpackerHandler_HQNode::reset()
|
||||
{
|
||||
_cached_scan_node_buf_pos = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
* HQNode Sample Node Handler
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
BEGIN_DATAUNPACKER_NS()
|
||||
|
||||
namespace unpacker {
|
||||
|
||||
class UnpackerHandler_HQNode : public IDataUnpackerHandler {
|
||||
public:
|
||||
UnpackerHandler_HQNode();
|
||||
virtual ~UnpackerHandler_HQNode();
|
||||
|
||||
virtual _u8 getSampleAnswerType() const;
|
||||
virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
|
||||
virtual void reset();
|
||||
virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
|
||||
|
||||
protected:
|
||||
std::vector<_u8> _cached_scan_node_buf;
|
||||
int _cached_scan_node_buf_pos;
|
||||
SlamtecLidarTimingDesc _cachedTimingDesc;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -0,0 +1,159 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
* Normal Sample Node Handler
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "../dataunnpacker_commondef.h"
|
||||
#include "../dataunpacker.h"
|
||||
#include "../dataunnpacker_internal.h"
|
||||
|
||||
|
||||
#include "handler_normalnode.h"
|
||||
|
||||
BEGIN_DATAUNPACKER_NS()
|
||||
|
||||
namespace unpacker{
|
||||
|
||||
|
||||
static _u64 _getSampleDelayOffsetInLegacyMode(const SlamtecLidarTimingDesc& timing)
|
||||
{
|
||||
// guess channel baudrate by LIDAR model ....
|
||||
const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200;
|
||||
|
||||
_u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_t) * 10 / channelBaudRate;
|
||||
|
||||
if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
|
||||
{
|
||||
tranmissionDelay = 100; //dummy value
|
||||
}
|
||||
|
||||
// center of the sample duration
|
||||
const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
|
||||
const _u64 sampleFilterDelay = timing.sample_duration_uS;
|
||||
|
||||
return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS;
|
||||
}
|
||||
|
||||
UnpackerHandler_NormalNode::UnpackerHandler_NormalNode()
|
||||
: _cached_scan_node_buf_pos(0)
|
||||
{
|
||||
_cached_scan_node_buf.resize(sizeof(rplidar_response_measurement_node_t));
|
||||
memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
|
||||
;}
|
||||
|
||||
UnpackerHandler_NormalNode::~UnpackerHandler_NormalNode()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
_u8 UnpackerHandler_NormalNode::getSampleAnswerType() const
|
||||
{
|
||||
return RPLIDAR_ANS_TYPE_MEASUREMENT;
|
||||
}
|
||||
|
||||
void UnpackerHandler_NormalNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
|
||||
{
|
||||
for (size_t pos = 0; pos < cnt; ++pos) {
|
||||
_u8 current_data = data[pos];
|
||||
switch (_cached_scan_node_buf_pos) {
|
||||
case 0: // expect the sync bit and its reverse in this byte
|
||||
{
|
||||
_u8 tmp = (current_data >> 1);
|
||||
if ((tmp ^ current_data) & 0x1) {
|
||||
// pass
|
||||
}
|
||||
else {
|
||||
continue;
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
case 1: // expect the highest bit to be 1
|
||||
{
|
||||
if (current_data & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
|
||||
// pass
|
||||
}
|
||||
else {
|
||||
_cached_scan_node_buf_pos = 0;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case sizeof(rplidar_response_measurement_node_t) - 1: // new data ready
|
||||
{
|
||||
_cached_scan_node_buf[sizeof(rplidar_response_measurement_node_t) - 1] = current_data;
|
||||
_cached_scan_node_buf_pos = 0;
|
||||
|
||||
rplidar_response_measurement_node_t* node = reinterpret_cast<rplidar_response_measurement_node_t*>(&_cached_scan_node_buf[0]);
|
||||
#ifdef _CPU_ENDIAN_BIG
|
||||
node->angle_q6_checkbit = le16_to_cpu(node->angle_q6_checkbit);
|
||||
node->distance_q2 = le16_to_cpu(node->distance_q2);
|
||||
#endif
|
||||
//cast node to rplidar_response_measurement_node_hq_t
|
||||
rplidar_response_measurement_node_hq_t hqNode;
|
||||
hqNode.angle_z_q14 = (((node->angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle
|
||||
hqNode.dist_mm_q2 = node->distance_q2;
|
||||
hqNode.flag = (node->sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field
|
||||
hqNode.quality = (node->sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255
|
||||
|
||||
|
||||
engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInLegacyMode(_cachedTimingDesc), &hqNode);
|
||||
continue;
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
_cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void UnpackerHandler_NormalNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
|
||||
{
|
||||
if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
|
||||
assert(size == sizeof(_cachedTimingDesc));
|
||||
_cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
|
||||
}
|
||||
}
|
||||
|
||||
void UnpackerHandler_NormalNode::reset()
|
||||
{
|
||||
_cached_scan_node_buf_pos = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* Slamtec LIDAR SDK
|
||||
*
|
||||
* Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
|
||||
* http://www.slamtec.com
|
||||
*
|
||||
*/
|
||||
|
||||
/*
|
||||
* Sample Data Unpacker System
|
||||
* Normal Sample Node Handler
|
||||
*/
|
||||
|
||||
/*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
||||
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
||||
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
||||
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
||||
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
||||
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
BEGIN_DATAUNPACKER_NS()
|
||||
|
||||
namespace unpacker{
|
||||
|
||||
class UnpackerHandler_NormalNode : public IDataUnpackerHandler {
|
||||
public:
|
||||
UnpackerHandler_NormalNode();
|
||||
virtual ~UnpackerHandler_NormalNode();
|
||||
|
||||
virtual _u8 getSampleAnswerType() const;
|
||||
virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
|
||||
virtual void reset();
|
||||
virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
|
||||
protected:
|
||||
std::vector<_u8> _cached_scan_node_buf;
|
||||
int _cached_scan_node_buf_pos;
|
||||
|
||||
SlamtecLidarTimingDesc _cachedTimingDesc;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
END_DATAUNPACKER_NS()
|
||||
Reference in New Issue
Block a user