diff --git a/CMakeLists.txt b/CMakeLists.txt
index ccb0ab4cfaa2b8d75b2a1886f3b5e1ba21d03c33..b5e0ed621692abc9c62c812bc29f3967d107d221 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@ if (BUILD_TESTING)
include(FetchTestData)
lidarview_fetch_test_data(
DATA_NAME "livox"
- COMMIT_SHA "57cce905ce8eb160096b080a1eef46e91ffea78f"
+ COMMIT_SHA "8d932109d882fbb80ca0c545a49c313bdfd2f512"
)
set(livox_test_data_dir "${FETCH_TEST_DATA_DIR}")
endif ()
diff --git a/LivoxDataFormat/CMakeLists.txt b/LivoxDataFormat/CMakeLists.txt
index 0dbec2eaca451608c044d2e33d46e1c6e42626ad..2fd847074eb37b3d7598fbba1072fe1d9edceedc 100644
--- a/LivoxDataFormat/CMakeLists.txt
+++ b/LivoxDataFormat/CMakeLists.txt
@@ -1,9 +1,15 @@
set(headers
LivoxPacketFormat.h
+ SDK1PacketFormat.h
SDK2PacketFormat.h
)
+set(sources
+ LivoxPacketFormat.cxx
+)
+
vtk_module_add_module(LivoxPlugin::LivoxDataFormat
- HEADER_ONLY
+ FORCE_STATIC
HEADERS ${headers}
+ SOURCES ${sources}
)
diff --git a/LivoxDataFormat/LivoxPacketFormat.cxx b/LivoxDataFormat/LivoxPacketFormat.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..2481d8816999f122d1e495cf265d4c207653dad8
--- /dev/null
+++ b/LivoxDataFormat/LivoxPacketFormat.cxx
@@ -0,0 +1,366 @@
+/*=========================================================================
+
+ Program: LidarView
+ Module: LivoxPacketFormat.cxx
+
+ Copyright (c) Kitware Inc.
+ All rights reserved.
+ See LICENSE or http://www.apache.org/licenses/LICENSE-2.0 for details.
+
+ This software is distributed WITHOUT ANY WARRANTY; without even
+ the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ PURPOSE. See the above copyright notice for more information.
+
+=========================================================================*/
+
+#include "LivoxPacketFormat.h"
+
+// STD
+#include <cmath>
+#include <cstring>
+#include <iostream>
+#include <string>
+
+namespace
+{
+typedef union
+{
+ uint8_t stamp_bytes[8];
+ int64_t stamp;
+} LdsStamp;
+
+// ============================================================================
+// ============================ Timestamps methods ============================
+// ============================================================================
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+double GetPointTimestamp(const livox::sdk1::EthernetPacket* pkt,
+ livox::DeviceType type,
+ uint32_t idx)
+{
+ double pktTimestamp = livox::GetEthPacketTimestamp(pkt->timestamp, 8) * ::MS_TO_US_FACTOR;
+ auto sdk1Type = static_cast<livox::sdk1::DeviceType>(type);
+ double ptsTimestamp = DataPointT::GetPointInterval(sdk1Type) * idx; // in microseconds (us)
+ return pktTimestamp + ptsTimestamp;
+}
+
+//-----------------------------------------------------------------------------
+double GetPointTimestamp(const livox::sdk2::EthernetPacket* pkt, uint32_t idx)
+{
+ double pktTimestamp = livox::GetEthPacketTimestamp(pkt->timestamp, 8) * ::MS_TO_US_FACTOR;
+ double ptsTimestamp = (static_cast<double>(pkt->timeInterval) / pkt->dotNum) * idx * 0.1; // in ns
+ return pktTimestamp + ptsTimestamp * ::NS_TO_US_FACTOR;
+}
+
+// ============================================================================
+// ========================== Get point info methods ==========================
+// ============================================================================
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetPointInfoNoTag(const livox::sdk1::EthernetPacket* pkt,
+ livox::LidarPoint& point,
+ uint32_t idx)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(pkt->data);
+ point.reflectivity = points[idx].reflectivity;
+ point.timestamp = ::GetPointTimestamp<DataPointT>(pkt, point.type, idx);
+}
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetPointInfo(const livox::sdk1::EthernetPacket* pkt, livox::LidarPoint& point, uint32_t idx)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(pkt->data);
+ point.reflectivity = points[idx].reflectivity;
+ point.timestamp = ::GetPointTimestamp<DataPointT>(pkt, point.type, idx);
+ point.tag = points[idx].tag;
+}
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetPointInfo(const livox::sdk1::EthernetPacket* pkt,
+ livox::LidarPoint& point,
+ uint32_t idx,
+ uint16_t iRet)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(pkt->data);
+ point.reflectivity = points[idx].data[iRet].reflectivity;
+ point.timestamp = ::GetPointTimestamp<DataPointT>(pkt, point.type, idx);
+ point.tag = points[idx].data[iRet].tag;
+}
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetPointInfo(const livox::sdk2::EthernetPacket* pkt, livox::LidarPoint& point, uint32_t idx)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(pkt->data);
+ point.reflectivity = points[idx].reflectivity;
+ point.tag = points[idx].tag;
+ point.timestamp = ::GetPointTimestamp(pkt, idx);
+}
+
+// ============================================================================
+// ======================== Get cartesian point methods =======================
+// ============================================================================
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetCartesianPoint(const DataPointT& data, double pos[3], double factor)
+{
+ pos[0] = static_cast<double>(data.x) * factor;
+ pos[1] = static_cast<double>(data.y) * factor;
+ pos[2] = static_cast<double>(data.z) * factor;
+}
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetCartesianPoint(const uint8_t* data,
+ double pos[3],
+ uint32_t idx,
+ double factor = ::MM_TO_M_FACTOR)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(data);
+ ::GetCartesianPoint(points[idx], pos, factor);
+}
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetCartesianPoint(const uint8_t* data, double pos[3], uint32_t idx, uint16_t iRet)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(data);
+ ::GetCartesianPoint(points[idx].data[iRet], pos, ::MM_TO_M_FACTOR);
+}
+
+// ============================================================================
+// ======================== Get spherical point methods =======================
+// ============================================================================
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetSphericalPoint(const uint8_t* data, double pos[3], uint32_t idx, uint32_t depth)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(data);
+
+ double rho = static_cast<double>(depth) * ::MM_TO_M_FACTOR;
+ double theta = static_cast<double>(points[idx].theta) / 100; // unit 0.01 degree
+ double phi = static_cast<double>(points[idx].phi) / 100; // unit 0.01 degree
+
+ pos[0] = rho * std::sin(phi) * std::cos(theta);
+ pos[1] = rho * std::sin(phi) * std::sin(theta);
+ pos[2] = rho * std::cos(phi);
+}
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetSphericalPoint(const uint8_t* data, double pos[3], uint32_t idx)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(data);
+ ::GetSphericalPoint<DataPointT>(data, pos, idx, points[idx].depth);
+}
+
+//-----------------------------------------------------------------------------
+template <typename DataPointT>
+void GetSphericalPoint(const uint8_t* data, double pos[3], uint32_t idx, uint16_t iRet)
+{
+ const auto* points = reinterpret_cast<const DataPointT*>(data);
+ ::GetSphericalPoint<DataPointT>(data, pos, idx, points[idx].data[iRet].depth);
+}
+}
+
+//-----------------------------------------------------------------------------
+std::string livox::GetLidarModelName(livox::DeviceType model)
+{
+ switch (model)
+ {
+ case livox::MID_40:
+ return "Mid-40";
+ case livox::TELE_15:
+ return "Tele-15";
+ case livox::HORIZON:
+ return "Horizon";
+ case livox::MID_70:
+ return "Mid-70";
+ case livox::AVIA:
+ return "Avia";
+ case livox::MID_360:
+ return "Mid-360";
+ case livox::HAP:
+ return "HAP";
+ default:
+ return "Unsupported Model";
+ }
+}
+
+//-----------------------------------------------------------------------------
+livox::SDKType livox::GetSDKType(DeviceType model)
+{
+ return static_cast<SDKType>(model == MID_360 || model == HAP);
+};
+
+//-----------------------------------------------------------------------------
+double livox::GetEthPacketTimestamp(const uint8_t* time_stamp, uint8_t size)
+{
+ ::LdsStamp time;
+ std::memcpy(time.stamp_bytes, time_stamp, size);
+ return time.stamp * ::NS_TO_MS_FACTOR;
+}
+
+//-----------------------------------------------------------------------------
+bool livox::IsValidPacket(const sdk1::EthernetPacket* pkt, uint32_t dataLength, bool isIMU)
+{
+ if (!pkt || dataLength < sizeof(sdk1::EthernetPacket))
+ {
+ return false;
+ }
+ if (isIMU == (pkt->dataType != sdk1::IMU_DATA))
+ {
+ return false;
+ }
+ uint16_t pointSize = 0;
+ switch (pkt->dataType)
+ {
+ case sdk1::CARTESIAN_POINT:
+ pointSize = sizeof(sdk1::RawPoint);
+ break;
+ case sdk1::SPHERICAL_POINT:
+ pointSize = sizeof(sdk1::SpherePoint);
+ break;
+ case sdk1::EXT_CARTESIAN_POINT:
+ pointSize = sizeof(sdk1::ExtendRawPoint);
+ break;
+ case sdk1::EXT_SPHERICAL_POINT:
+ pointSize = sizeof(sdk1::ExtendSpherePoint);
+ break;
+ case sdk1::EXT_CARTESIAN_POINT_DUAL:
+ pointSize = sizeof(sdk1::DualExtendRawPoint);
+ break;
+ case sdk1::EXT_SPHERICAL_POINT_DUAL:
+ pointSize = sizeof(sdk1::DualExtendSpherePoint);
+ break;
+ case sdk1::IMU_DATA:
+ pointSize = sizeof(ImuRawPoint);
+ break;
+ case sdk1::EXT_CARTESIAN_POINT_TRIPLE:
+ pointSize = sizeof(sdk1::TripleExtendRawPoint);
+ break;
+ case sdk1::EXT_SPHERICAL_POINT_TRIPLE:
+ pointSize = sizeof(sdk1::TripleExtendSpherePoint);
+ break;
+ default:
+ return false;
+ }
+ uint32_t theoreticalSize = pointSize * sdk1::NB_POINTS_PER_PACKETS[pkt->dataType];
+ // Remove size of fake member data in EthernetPacket
+ theoreticalSize += sizeof(sdk1::EthernetPacket) - sizeof(uint8_t);
+ return theoreticalSize == dataLength;
+}
+
+//-----------------------------------------------------------------------------
+bool livox::IsValidPacket(const sdk2::EthernetPacket* pkt, uint32_t dataLength, bool isIMU)
+{
+ if (!pkt || dataLength < sizeof(sdk2::EthernetPacket) || pkt->length != dataLength)
+ {
+ return false;
+ }
+ if (isIMU == (pkt->dataType != sdk2::IMU_DATA))
+ {
+ return false;
+ }
+ uint16_t pointSize = 0;
+ switch (pkt->dataType)
+ {
+ case sdk2::IMU_DATA:
+ pointSize = sizeof(ImuRawPoint);
+ break;
+ case sdk2::CARTESIAN_COORDINATE_HIGH:
+ pointSize = sizeof(sdk2::CartesianHighRawPoint);
+ break;
+ case sdk2::CARTESIAN_COORDINATE_LOW:
+ pointSize = sizeof(sdk2::CartesianLowRawPoint);
+ break;
+ case sdk2::SPHERICAL_COORDINATE:
+ pointSize = sizeof(sdk2::SpherePoint);
+ break;
+ default:
+ return false;
+ }
+ uint32_t theoreticalSize = pointSize * pkt->dotNum;
+ // Remove size of fake member data in EthernetPacket
+ theoreticalSize += sizeof(sdk2::EthernetPacket) - sizeof(uint8_t);
+ return theoreticalSize == dataLength;
+}
+
+//-----------------------------------------------------------------------------
+void livox::ParsePoint(const sdk1::EthernetPacket* pkt,
+ LidarPoint& point,
+ uint32_t idx,
+ uint16_t iRet)
+{
+ switch (pkt->dataType)
+ {
+ case sdk1::CARTESIAN_POINT:
+ ::GetCartesianPoint<sdk1::RawPoint>(pkt->data, point.position, idx);
+ ::GetPointInfoNoTag<sdk1::RawPoint>(pkt, point, idx);
+ break;
+
+ case sdk1::SPHERICAL_POINT:
+ ::GetSphericalPoint<sdk1::SpherePoint>(pkt->data, point.position, idx);
+ ::GetPointInfoNoTag<sdk1::SpherePoint>(pkt, point, idx);
+ break;
+
+ case sdk1::EXT_CARTESIAN_POINT:
+ ::GetCartesianPoint<sdk1::ExtendRawPoint>(pkt->data, point.position, idx);
+ ::GetPointInfo<sdk1::ExtendRawPoint>(pkt, point, idx);
+ break;
+
+ case sdk1::EXT_SPHERICAL_POINT:
+ ::GetSphericalPoint<sdk1::ExtendSpherePoint>(pkt->data, point.position, idx);
+ ::GetPointInfo<sdk1::ExtendSpherePoint>(pkt, point, idx);
+ break;
+
+ case sdk1::EXT_CARTESIAN_POINT_DUAL:
+ ::GetCartesianPoint<sdk1::DualExtendRawPoint>(pkt->data, point.position, idx, iRet);
+ ::GetPointInfo<sdk1::DualExtendRawPoint>(pkt, point, idx, iRet);
+ break;
+
+ case sdk1::EXT_SPHERICAL_POINT_DUAL:
+ ::GetSphericalPoint<sdk1::DualExtendSpherePoint>(pkt->data, point.position, idx, iRet);
+ ::GetPointInfo<sdk1::DualExtendSpherePoint>(pkt, point, idx, iRet);
+ break;
+
+ case sdk1::EXT_CARTESIAN_POINT_TRIPLE:
+ ::GetCartesianPoint<sdk1::TripleExtendRawPoint>(pkt->data, point.position, idx, iRet);
+ ::GetPointInfo<sdk1::TripleExtendRawPoint>(pkt, point, idx, iRet);
+ break;
+
+ case sdk1::EXT_SPHERICAL_POINT_TRIPLE:
+ ::GetSphericalPoint<sdk1::TripleExtendSpherePoint>(pkt->data, point.position, idx, iRet);
+ ::GetPointInfo<sdk1::TripleExtendSpherePoint>(pkt, point, idx, iRet);
+ break;
+ }
+}
+
+//-----------------------------------------------------------------------------
+void livox::ParsePoint(const sdk2::EthernetPacket* pkt, LidarPoint& point, uint32_t idx)
+{
+ switch (pkt->dataType)
+ {
+ case sdk2::CARTESIAN_COORDINATE_HIGH:
+ ::GetCartesianPoint<sdk2::CartesianHighRawPoint>(pkt->data, point.position, idx);
+ ::GetPointInfo<sdk2::CartesianHighRawPoint>(pkt, point, idx);
+ break;
+
+ case sdk2::CARTESIAN_COORDINATE_LOW:
+ ::GetCartesianPoint<sdk2::CartesianLowRawPoint>(
+ pkt->data, point.position, idx, ::CM_TO_M_FACTOR);
+ ::GetPointInfo<sdk2::CartesianLowRawPoint>(pkt, point, idx);
+ break;
+
+ case sdk2::SPHERICAL_COORDINATE:
+ ::GetSphericalPoint<sdk2::SpherePoint>(pkt->data, point.position, idx);
+ ::GetPointInfo<sdk2::SpherePoint>(pkt, point, idx);
+ break;
+ }
+}
diff --git a/LivoxDataFormat/LivoxPacketFormat.h b/LivoxDataFormat/LivoxPacketFormat.h
index ed3d6f446569936c06efad7d712fe9be60c2c77f..b646d0c957869f51d40899734485dd765607b277 100644
--- a/LivoxDataFormat/LivoxPacketFormat.h
+++ b/LivoxDataFormat/LivoxPacketFormat.h
@@ -16,22 +16,60 @@
#ifndef LivoxPacketFormat_h
#define LivoxPacketFormat_h
+// Local
+#include "SDK1PacketFormat.h"
#include "SDK2PacketFormat.h"
-// Instructs the compiler to pack structure members
-#pragma pack(1)
+// STD
+#include <string>
+
+namespace
+{
+constexpr double NS_TO_MS_FACTOR = 1e-6;
+constexpr double MS_TO_US_FACTOR = 1e3;
+constexpr double NS_TO_US_FACTOR = 1e-3;
+constexpr double MS_TO_S_FACTOR = 1e-3;
+
+constexpr double MM_TO_M_FACTOR = 1e-3;
+constexpr double CM_TO_M_FACTOR = 1e-2;
+}
namespace livox
{
+constexpr const char* DEFAULT_TAG_NAMES[3] = { "spatial_confidence",
+ "intensity_confidence",
+ "return_number" };
+constexpr uint8_t NB_LASER_PER_DEVICE[7] = { 1, 1, 1, 1, 6, 4, 6 };
+
enum DeviceType
{
- MID_40 = 0,
- TELE_15 = 1,
- HORIZON = 2,
- MID_70 = 3,
- AVIA = 4,
- MID_360 = 5,
- HAP = 6
+ MID_40 = sdk1::MID_40,
+ TELE_15 = sdk1::TELE_15,
+ HORIZON = sdk1::HORIZON,
+ MID_70 = sdk1::MID_70,
+ AVIA = sdk1::AVIA,
+ MID_360 = sdk2::MID_360,
+ HAP = sdk2::HAP
+};
+
+enum SDKType
+{
+ SDK1 = 0,
+ SDK2 = 1
+};
+
+// Instructs the compiler to pack structure members
+#pragma pack(push, 1)
+
+/** Internal representation of point information */
+struct LidarPoint
+{
+ double position[3];
+ uint8_t reflectivity;
+ uint8_t tag;
+ uint8_t laserId;
+ double timestamp;
+ DeviceType type;
};
struct ImuRawPoint
@@ -44,6 +82,39 @@ struct ImuRawPoint
float accZ; /**< Accelerometer Z axis, Unit:g */
};
+#pragma pack(pop)
+
+/**
+ * Return a string containing the LiDARs model name.
+ */
+std::string GetLidarModelName(livox::DeviceType model);
+
+/**
+ * Return the SDK used for a specific device (sdk1 or sdk2)
+ */
+SDKType GetSDKType(DeviceType model);
+
+///@{
+/**
+ * Return true if the packet is considered valid.
+ * Check if type is coherent with packet size.
+ */
+bool IsValidPacket(const sdk1::EthernetPacket* pkt, uint32_t dataLength, bool isIMU = false);
+bool IsValidPacket(const sdk2::EthernetPacket* pkt, uint32_t dataLength, bool isIMU = false);
+///@}
+
+/**
+ * Return the packet timestamp in milliseconds (ms)
+ */
+double GetEthPacketTimestamp(const uint8_t* time_stamp, uint8_t size);
+
+///@{
+/**
+ * Methods specialized for sdk1 and sdk2 to parse point depending of the pkt data type.
+ */
+void ParsePoint(const sdk1::EthernetPacket* pkt, LidarPoint& point, uint32_t idx, uint16_t iRet);
+void ParsePoint(const sdk2::EthernetPacket* pkt, LidarPoint& point, uint32_t idx);
+///@}
};
#endif // LivoxPacketFormat_h
diff --git a/LivoxDataFormat/SDK1PacketFormat.h b/LivoxDataFormat/SDK1PacketFormat.h
new file mode 100644
index 0000000000000000000000000000000000000000..2667a39ab9799dd80cf4853da11cb76065835e02
--- /dev/null
+++ b/LivoxDataFormat/SDK1PacketFormat.h
@@ -0,0 +1,187 @@
+/*=========================================================================
+
+ Program: LidarView
+ Module: SDK1PacketFormat.h
+
+ Copyright (c) Kitware Inc.
+ All rights reserved.
+ See LICENSE or http://www.apache.org/licenses/LICENSE-2.0 for details.
+
+ This software is distributed WITHOUT ANY WARRANTY; without even
+ the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ PURPOSE. See the above copyright notice for more information.
+
+=========================================================================*/
+
+#ifndef SDK1PacketFormat_h
+#define SDK1PacketFormat_h
+
+#include <cstdint>
+
+// Instructs the compiler to pack structure members
+#pragma pack(push, 1)
+
+namespace livox
+{
+namespace sdk1
+{
+
+enum DeviceType
+{
+ MID_40 = 0,
+ TELE_15 = 1,
+ HORIZON = 2,
+ MID_70 = 3,
+ AVIA = 4
+};
+
+/** Point cloud packet. */
+struct EthernetPacket
+{
+ uint8_t version; /**< Packet protocol version. */
+ uint8_t slot; /**< Slot number used for connecting LiDAR. */
+ uint8_t id; /**< LiDAR id. */
+ uint8_t rsvd; /**< Reserved. */
+ uint32_t errCode; /**< Device error status indicator information. */
+ uint8_t timestampType; /**< Timestamp type. */
+ /** Point cloud coordinate format, refer to \ref PointDataType . */
+ uint8_t dataType;
+ uint8_t timestamp[8]; /**< Nanosecond or UTC format timestamp. */
+ uint8_t data[1]; /**< Point cloud data. */
+};
+
+constexpr double INTERVAL_100KHZ = 1. / 0.1; // in us for 100,000 points/s
+constexpr double INTERVAL_200KHZ = 1. / 0.2; // in us for 200,000 points/s
+constexpr double INTERVAL_240KHZ = 1. / 0.24; // in us for 240,000 points/s
+constexpr double INTERVAL_480KHZ = 1. / 0.48; // in us for 480,000 points/s
+constexpr double INTERVAL_720KHZ = 1. / 0.72; // in us for 720,000 points/s
+constexpr double INTERVAL_200HZ = 1. / 0.0002; // in us for IMU
+
+/** Nb of point data per packet for each data type */
+constexpr uint32_t NB_POINTS_PER_PACKETS[9] = { 100, 100, 96, 96, 48, 48, 1, 30, 30 };
+
+/** Nb of returns for each data type */
+constexpr uint16_t NUMBER_OF_RETURNS[9] = { 1, 1, 1, 1, 2, 2, 0, 3, 3 };
+
+/** Point data type. */
+enum PointDataType
+{
+ CARTESIAN_POINT = 0x00, /**< Mid-40 */
+ SPHERICAL_POINT = 0x01, /**< Mid-40 */
+ EXT_CARTESIAN_POINT = 0x02, /**< Horizon, Tele-15, Mid-70, Avia */
+ EXT_SPHERICAL_POINT = 0x03, /**< Horizon, Tele-15, Mid-70, Avia */
+ EXT_CARTESIAN_POINT_DUAL = 0x04, /**< Horizon, Tele-15, Mid-70, Avia */
+ EXT_SPHERICAL_POINT_DUAL = 0x05, /**< Horizon, Tele-15, Mid-70, Avia */
+ IMU_DATA = 0x06, /**< Horizon, Tele-15, Avia */
+ EXT_CARTESIAN_POINT_TRIPLE = 0x07, /**< Avia */
+ EXT_SPHERICAL_POINT_TRIPLE = 0x08, /**< Avia */
+};
+
+/** #0 Cartesian coordinate format. */
+struct RawPoint
+{
+ int32_t x; /**< X axis, Unit:mm */
+ int32_t y; /**< Y axis, Unit:mm */
+ int32_t z; /**< Z axis, Unit:mm */
+ uint8_t reflectivity; /**< Reflectivity */
+
+ static double GetPointInterval(DeviceType) { return INTERVAL_100KHZ; };
+};
+
+/** #1 Spherical coordinate format. */
+struct SpherePoint
+{
+ uint32_t depth; /**< Depth, Unit: mm */
+ uint16_t theta; /**< Zenith angle[0, 18000], Unit: 0.01 degree */
+ uint16_t phi; /**< Azimuth[0, 36000], Unit: 0.01 degree */
+ uint8_t reflectivity; /**< Reflectivity */
+
+ static double GetPointInterval(DeviceType) { return INTERVAL_100KHZ; };
+};
+
+/** #2 Extend cartesian coordinate format. */
+struct ExtendRawPoint
+{
+ int32_t x; /**< X axis, Unit:mm */
+ int32_t y; /**< Y axis, Unit:mm */
+ int32_t z; /**< Z axis, Unit:mm */
+ uint8_t reflectivity; /**< Reflectivity */
+ uint8_t tag; /**< Tag */
+
+ static double GetPointInterval(DeviceType type)
+ {
+ return type == MID_70 ? INTERVAL_100KHZ : INTERVAL_240KHZ;
+ };
+};
+
+/** #3 Extend spherical coordinate format. */
+struct ExtendSpherePoint
+{
+ uint32_t depth; /**< Depth, Unit: mm */
+ uint16_t theta; /**< Zenith angle[0, 18000], Unit: 0.01 degree */
+ uint16_t phi; /**< Azimuth[0, 36000], Unit: 0.01 degree */
+ uint8_t reflectivity; /**< Reflectivity */
+ uint8_t tag; /**< Tag */
+
+ static double GetPointInterval(DeviceType type)
+ {
+ return type == MID_70 ? INTERVAL_100KHZ : INTERVAL_240KHZ;
+ };
+};
+
+/** #4 Dual extend cartesian coordinate format. */
+struct DualExtendRawPoint
+{
+ ExtendRawPoint data[2];
+
+ static double GetPointInterval(DeviceType type)
+ {
+ return type == MID_70 ? INTERVAL_200KHZ : INTERVAL_480KHZ;
+ };
+};
+
+/** Sphere data for dual and triple points */
+struct SphereData
+{
+ uint32_t depth; /**< Depth, Unit: mm */
+ uint8_t reflectivity; /**< Reflectivity */
+ uint8_t tag; /**< Tag */
+};
+
+/** #5 Dual extend spherical coordinate format. */
+struct DualExtendSpherePoint
+{
+ uint16_t theta; /**< Zenith angle[0, 18000], Unit: 0.01 degree */
+ uint16_t phi; /**< Azimuth[0, 36000], Unit: 0.01 degree */
+ SphereData data[2]; /**< See SphereData struct */
+
+ static double GetPointInterval(DeviceType type)
+ {
+ return type == MID_70 ? INTERVAL_200KHZ : INTERVAL_480KHZ;
+ };
+};
+
+/** #7 Triple extend cartesian coordinate format. */
+struct TripleExtendRawPoint
+{
+ ExtendRawPoint data[3];
+
+ static double GetPointInterval(DeviceType) { return INTERVAL_720KHZ; };
+};
+
+/** #8 Triple extend spherical coordinate format. */
+struct TripleExtendSpherePoint
+{
+ uint16_t theta; /**< Zenith angle[0, 18000], Unit: 0.01 degree */
+ uint16_t phi; /**< Azimuth[0, 36000], Unit: 0.01 degree */
+ SphereData data[3]; /**< See SphereData struct */
+
+ static double GetPointInterval(DeviceType) { return INTERVAL_720KHZ; };
+};
+
+};
+};
+
+#pragma pack(pop)
+
+#endif // SDK1PacketFormat_h
diff --git a/LivoxDataFormat/SDK2PacketFormat.h b/LivoxDataFormat/SDK2PacketFormat.h
index e12462b1ffa1c738ea72ba8acc9c6ac9ce695c14..0a953be4eda24adfa63d0b91e124f380f80855cc 100644
--- a/LivoxDataFormat/SDK2PacketFormat.h
+++ b/LivoxDataFormat/SDK2PacketFormat.h
@@ -19,13 +19,21 @@
#include <cstdint>
// Instructs the compiler to pack structure members
-#pragma pack(1)
+#pragma pack(push, 1)
namespace livox
{
namespace sdk2
{
+enum DeviceType
+{
+ MID_360 = 5,
+ HAP = 6
+};
+
+constexpr const char* MID360_TAG_NAMES[3] = { "tag_glue", "tag_particles", "tag_other" };
+
struct EthernetPacket
{
uint8_t version;
@@ -80,4 +88,6 @@ enum PointDataType
};
};
+#pragma pack(pop)
+
#endif // SDK2PacketFormat_h
diff --git a/LivoxPacketInterpreters/LivoxLidarReader.xml b/LivoxPacketInterpreters/LivoxLidarReader.xml
index 416ac841cdcd9e701cac78b96b4e3c5dd1927505..a2dba3598586e6056c121ac569d552c5a95eeddc 100644
--- a/LivoxPacketInterpreters/LivoxLidarReader.xml
+++ b/LivoxPacketInterpreters/LivoxLidarReader.xml
@@ -20,6 +20,11 @@
</Documentation>
<Hints>
<PropertyWidgetDecorator type="ConditionalDefaultValueDecorator" property="LidarModel">
+ <Entry default_value="50002" target_value="Mid-40" />
+ <Entry default_value="50002" target_value="Tele-15" />
+ <Entry default_value="50002" target_value="Horizon" />
+ <Entry default_value="50002" target_value="Mid-70" />
+ <Entry default_value="50002" target_value="Avia" />
<Entry default_value="56301" target_value="Mid-360" />
<Entry default_value="57000" target_value="HAP" />
</PropertyWidgetDecorator>
diff --git a/LivoxPacketInterpreters/LivoxLidarStream.xml b/LivoxPacketInterpreters/LivoxLidarStream.xml
index 9b62ca1d5a6822f9b69652f2f30aaced61c35239..14db7e7143e6dae295f3177b3cfe942191e3dda6 100644
--- a/LivoxPacketInterpreters/LivoxLidarStream.xml
+++ b/LivoxPacketInterpreters/LivoxLidarStream.xml
@@ -21,6 +21,11 @@
</Documentation>
<Hints>
<PropertyWidgetDecorator type="ConditionalDefaultValueDecorator" property="LidarModel">
+ <Entry default_value="50002" target_value="Mid-40" />
+ <Entry default_value="50002" target_value="Tele-15" />
+ <Entry default_value="50002" target_value="Horizon" />
+ <Entry default_value="50002" target_value="Mid-70" />
+ <Entry default_value="50002" target_value="Avia" />
<Entry default_value="56301" target_value="Mid-360" />
<Entry default_value="57000" target_value="HAP" />
</PropertyWidgetDecorator>
diff --git a/LivoxPacketInterpreters/LivoxPacketInterpreter.xml b/LivoxPacketInterpreters/LivoxPacketInterpreter.xml
index 529948974ede9b25d1ee8e742578de0d3610707c..7f1ea8a88bc6bac595ea8ef71dce90629ec0a545 100644
--- a/LivoxPacketInterpreters/LivoxPacketInterpreter.xml
+++ b/LivoxPacketInterpreters/LivoxPacketInterpreter.xml
@@ -10,8 +10,13 @@
command="SetLidarModel"
number_of_elements="1"
override="1"
- default_values="9">
+ default_values="5">
<EnumerationDomain name="enum">
+ <Entry value="0" text="Mid-40" />
+ <Entry value="1" text="Tele-15" />
+ <Entry value="2" text="Horizon" />
+ <Entry value="3" text="Mid-70" />
+ <Entry value="4" text="Avia" />
<Entry value="5" text="Mid-360" />
<Entry value="6" text="HAP" />
</EnumerationDomain>
@@ -23,11 +28,21 @@
<IntVectorProperty name="PublishInterval"
command="SetPublishInterval"
number_of_elements="1"
- default_values="100"
- panel_visibility="advanced">
+ default_values="100">
<Documentation>
Interval representing a "time length" of a frame (in ms).
</Documentation>
+ <Hints>
+ <PropertyWidgetDecorator type="ConditionalDefaultValueDecorator" property="LidarModel">
+ <Entry default_value="200" target_value="Mid-40" />
+ <Entry default_value="200" target_value="Tele-15" />
+ <Entry default_value="200" target_value="Horizon" />
+ <Entry default_value="200" target_value="Mid-70" />
+ <Entry default_value="100" target_value="Avia" />
+ <Entry default_value="100" target_value="Mid-360" />
+ <Entry default_value="100" target_value="HAP" />
+ </PropertyWidgetDecorator>
+ </Hints>
</IntVectorProperty>
</Proxy>
</ProxyGroup>
diff --git a/LivoxPacketInterpreters/vtkLivoxPacketInterpreter.cxx b/LivoxPacketInterpreters/vtkLivoxPacketInterpreter.cxx
index fb10656c8385688325d0389020661cc2f93d59fe..d6c6895d7e6bf0001bc4e94a0bb244ea03c1b5d5 100644
--- a/LivoxPacketInterpreters/vtkLivoxPacketInterpreter.cxx
+++ b/LivoxPacketInterpreters/vtkLivoxPacketInterpreter.cxx
@@ -28,54 +28,8 @@
namespace
{
+typedef livox::sdk1::EthernetPacket SDK1Packet;
typedef livox::sdk2::EthernetPacket SDK2Packet;
-
-constexpr const char* MID360_TAG_NAMES[3] = { "tag_glue", "tag_particles", "tag_other" };
-constexpr const char* HAP_TAG_NAMES[3] = { "spatial_confidence",
- "intensity_confidence",
- "return_number" };
-
-typedef union
-{
- uint8_t stamp_bytes[8];
- int64_t stamp;
-} LdsStamp;
-
-constexpr double NS_TO_MS_FACTOR = 1e-6;
-constexpr double MS_TO_US_FACTOR = 1e3;
-constexpr double NS_TO_US_FACTOR = 1e-3;
-
-//-----------------------------------------------------------------------------
-/**
- * Return the packet timestamp in milliseconds
- */
-double GetEthPacketTimestamp(const uint8_t* time_stamp, uint8_t size)
-{
- LdsStamp time;
- memcpy(time.stamp_bytes, time_stamp, size);
- return time.stamp * ::NS_TO_MS_FACTOR;
-}
-
-double GetPointTimestamp(const SDK2Packet* pkt, unsigned int idx)
-{
- double pktTimestamp = ::GetEthPacketTimestamp(pkt->timestamp, 8) * ::MS_TO_US_FACTOR;
- double ptsTimestamp = (static_cast<double>(pkt->timeInterval) / pkt->dotNum) * idx * 0.1; // in ns
- return pktTimestamp + ptsTimestamp * ::NS_TO_US_FACTOR;
-}
-
-//-----------------------------------------------------------------------------
-std::string GetLidarModelName(livox::DeviceType model)
-{
- switch (model)
- {
- case livox::MID_360:
- return "Mid-360";
- case livox::HAP:
- return "HAP";
- default:
- return "Unsupported Model";
- }
-}
}
//-----------------------------------------------------------------------------
@@ -83,9 +37,11 @@ class vtkLivoxPacketInterpreter::vtkInternals
{
public:
//-----------------------------------------------------------------------------
- bool DoSplitPacket(const SDK2Packet* pkt, uint64_t interval)
+ template <typename PacketT>
+ bool DoSplitPacket(unsigned char const* data, uint64_t interval)
{
- double timestamp = ::GetEthPacketTimestamp(pkt->timestamp, 8);
+ const PacketT* pkt = reinterpret_cast<const PacketT*>(data);
+ double timestamp = livox::GetEthPacketTimestamp(pkt->timestamp, 8);
if (this->IsFirstTime || this->LastTimeStamp > timestamp ||
timestamp - this->LastTimeStamp > interval * 2U)
{
@@ -103,93 +59,47 @@ public:
}
//-----------------------------------------------------------------------------
- template <typename T>
- void FillPoints(double* pos, const T& point, double timestamp, uint8_t laserId)
- {
- if (std::isnan(pos[0]) || std::isnan(pos[1]) || std::isnan(pos[2]))
- {
- return;
- }
-
- double distance =
- std::sqrt(std::pow(0 - pos[0], 2) + std::pow(0 - pos[1], 2) + std::pow(0 - pos[2], 2));
-
- // Skip wrong points
- if (distance <= 0.1 || distance > 500.0)
- {
- return;
- }
-
- this->Points->InsertNextPoint(pos);
- InsertNextValueIfNotNull(this->PointsX, pos[0]);
- InsertNextValueIfNotNull(this->PointsY, pos[1]);
- InsertNextValueIfNotNull(this->PointsZ, pos[2]);
- InsertNextValueIfNotNull(this->Intensity, point.reflectivity);
- InsertNextValueIfNotNull(this->LaserId, laserId);
- InsertNextValueIfNotNull(this->Timestamp, timestamp);
- InsertNextValueIfNotNull(this->Distance, distance);
- InsertNextValueIfNotNull(this->FirstTag, !!(point.tag & 0x00) + !!(point.tag & 0x01));
- InsertNextValueIfNotNull(this->SecondTag, !!(point.tag & 0x02) + !!(point.tag & 0x03));
- InsertNextValueIfNotNull(this->ThirdTag, !!(point.tag & 0x04) + !!(point.tag & 0x05));
- }
-
- //-----------------------------------------------------------------------------
- void FillHighRawPoints(const SDK2Packet* pkt)
+ template <typename PacketT>
+ bool PreProcessPacket(unsigned char const* data, uint64_t interval, double& outLidarDataTime)
{
- const auto* points = reinterpret_cast<const livox::sdk2::CartesianHighRawPoint*>(pkt->data);
-
- for (uint32_t i = 0; i < pkt->dotNum; i++)
- {
- // import unit mm
- double pos[3] = { static_cast<double>(points[i].x) / 1000,
- static_cast<double>(points[i].y) / 1000,
- static_cast<double>(points[i].z) / 1000 };
- double ptsTimestamp = ::GetPointTimestamp(pkt, i);
- uint8_t laserId = this->GetLaserId(i);
- this->FillPoints(pos, points[i], ptsTimestamp, laserId);
- }
+ const PacketT* pkt = reinterpret_cast<const PacketT*>(data);
+ outLidarDataTime = livox::GetEthPacketTimestamp(pkt->timestamp, 8) * ::MS_TO_S_FACTOR;
+ return this->DoSplitPacket<PacketT>(data, interval);
}
//-----------------------------------------------------------------------------
- void FillLowRawPoints(const SDK2Packet* pkt)
+ void ProcessSDK1Packet(unsigned char const* data)
{
- const auto* points = reinterpret_cast<const livox::sdk2::CartesianLowRawPoint*>(pkt->data);
-
- for (uint32_t i = 0; i < pkt->dotNum; i++)
+ const SDK1Packet* pkt = reinterpret_cast<const SDK1Packet*>(data);
+ livox::LidarPoint point;
+ point.type = this->LidarModel;
+ for (uint32_t idx = 0; idx < livox::sdk1::NB_POINTS_PER_PACKETS[pkt->dataType]; idx++)
{
- // import unit cm
- double pos[3] = { static_cast<double>(points[i].x) / 100,
- static_cast<double>(points[i].y) / 100,
- static_cast<double>(points[i].z) / 100 };
- double ptsTimestamp = ::GetPointTimestamp(pkt, i);
- uint8_t laserId = this->GetLaserId(i);
- this->FillPoints(pos, points[i], ptsTimestamp, laserId);
+ point.laserId = this->GetLaserId(idx);
+ for (uint16_t iRet = 0; iRet < livox::sdk1::NUMBER_OF_RETURNS[pkt->dataType]; iRet++)
+ {
+ livox::ParsePoint(pkt, point, idx, iRet);
+ this->FillPoints(point);
+ }
}
}
//-----------------------------------------------------------------------------
- void FillSpherePoints(const SDK2Packet* pkt)
+ void ProcessSDK2Packet(unsigned char const* data)
{
- const auto* points = reinterpret_cast<const livox::sdk2::SpherePoint*>(pkt->data);
-
- for (uint32_t i = 0; i < pkt->dotNum; i++)
+ const SDK2Packet* pkt = reinterpret_cast<const SDK2Packet*>(data);
+ livox::LidarPoint point;
+ point.type = this->LidarModel;
+ for (uint32_t idx = 0; idx < pkt->dotNum; idx++)
{
- double rho = static_cast<double>(points[i].depth) / 1000; // import unit mm
- double theta = static_cast<double>(points[i].theta) / 100; // import unit 0.01 degree
- double phi = static_cast<double>(points[i].phi) / 100; // import unit 0.01 degree
-
- double pos[3];
- pos[0] = rho * std::sin(phi) * std::cos(theta);
- pos[1] = rho * std::sin(phi) * std::sin(theta);
- pos[2] = rho * std::cos(phi);
- double ptsTimestamp = ::GetPointTimestamp(pkt, i);
- uint8_t laserId = this->GetLaserId(i);
- this->FillPoints(pos, points[i], ptsTimestamp, laserId);
+ point.laserId = this->GetLaserId(idx);
+ livox::ParsePoint(pkt, point, idx);
+ this->FillPoints(point);
}
}
//-----------------------------------------------------------------------------
- const char* GetTagNames(uint8_t tagIdx)
+ const char* GetTagName(uint8_t tagIdx)
{
if (tagIdx >= 3)
{
@@ -197,30 +107,59 @@ public:
}
switch (this->LidarModel)
{
- case livox::HAP:
- return ::HAP_TAG_NAMES[tagIdx];
-
case livox::MID_360:
+ return livox::sdk2::MID360_TAG_NAMES[tagIdx];
+
default:
- return ::MID360_TAG_NAMES[tagIdx];
+ return livox::DEFAULT_TAG_NAMES[tagIdx];
}
}
private:
//-----------------------------------------------------------------------------
- uint8_t GetLaserId(uint32_t pointIdx)
+ void FillPoints(const livox::LidarPoint& point)
{
- switch (this->LidarModel)
+ if (std::isnan(point.position[0]) || std::isnan(point.position[1]) ||
+ std::isnan(point.position[2]))
+ {
+ return;
+ }
+
+ double distance = std::sqrt(std::pow(0 - point.position[0], 2) +
+ std::pow(0 - point.position[1], 2) + std::pow(0 - point.position[2], 2));
+
+ // Skip wrong points
+ if (distance <= 0.1 || distance > 500.0)
{
- case livox::HAP:
- return pointIdx % 6;
+ return;
+ }
- case livox::MID_360:
- default:
- return pointIdx % 4;
+ this->Points->InsertNextPoint(point.position);
+ InsertNextValueIfNotNull(this->PointsX, point.position[0]);
+ InsertNextValueIfNotNull(this->PointsY, point.position[1]);
+ InsertNextValueIfNotNull(this->PointsZ, point.position[2]);
+ InsertNextValueIfNotNull(this->Intensity, point.reflectivity);
+ InsertNextValueIfNotNull(this->LaserId, point.laserId);
+ InsertNextValueIfNotNull(this->Timestamp, point.timestamp);
+ InsertNextValueIfNotNull(this->Distance, distance);
+
+ if (this->LidarModel != livox::MID_40)
+ {
+ InsertNextValueIfNotNull(this->FirstTag, BF_GET(point.tag, 0, 2));
+ InsertNextValueIfNotNull(this->SecondTag, BF_GET(point.tag, 2, 2));
+ }
+ if (this->LidarModel != livox::MID_40 && this->LidarModel != livox::HAP)
+ {
+ InsertNextValueIfNotNull(this->ThirdTag, BF_GET(point.tag, 4, 2));
}
}
+ //-----------------------------------------------------------------------------
+ uint8_t GetLaserId(uint32_t pointIdx)
+ {
+ return pointIdx % livox::NB_LASER_PER_DEVICE[this->LidarModel];
+ }
+
public:
bool IsFirstTime = true;
double LastTimeStamp;
@@ -255,22 +194,15 @@ vtkLivoxPacketInterpreter::vtkLivoxPacketInterpreter()
//-----------------------------------------------------------------------------
bool vtkLivoxPacketInterpreter::IsLidarPacket(unsigned char const* data, unsigned int dataLength)
{
- const SDK2Packet* dataPacket = reinterpret_cast<const SDK2Packet*>(data);
-
- if (!dataPacket)
+ switch (livox::GetSDKType(this->Internals->LidarModel))
{
- return false;
- }
+ case livox::SDK1:
+ return livox::IsValidPacket(reinterpret_cast<const SDK1Packet*>(data), dataLength);
- if (dataPacket->length != dataLength)
- {
- return false;
- }
- if (dataPacket->dataType == livox::sdk2::IMU_DATA)
- {
- return false;
+ case livox::SDK2:
+ return livox::IsValidPacket(reinterpret_cast<const SDK2Packet*>(data), dataLength);
}
- return true;
+ return false;
}
//-----------------------------------------------------------------------------
@@ -305,9 +237,15 @@ vtkSmartPointer<vtkPolyData> vtkLivoxPacketInterpreter::CreateNewEmptyFrame(vtkI
InitArrayForPolyData(false, internals->Timestamp, "timestamp", nbrOfPoints, prereservedNbrOfPoints, polyData);
InitArrayForPolyData(false, internals->LaserId, "laser_id", nbrOfPoints, prereservedNbrOfPoints, polyData);
InitArrayForPolyData(false, internals->Distance, "distance_m", nbrOfPoints, prereservedNbrOfPoints, polyData);
- InitArrayForPolyData(true, internals->FirstTag, internals->GetTagNames(0), nbrOfPoints, prereservedNbrOfPoints, polyData, this->EnableAdvancedArrays);
- InitArrayForPolyData(true, internals->SecondTag, internals->GetTagNames(1), nbrOfPoints, prereservedNbrOfPoints, polyData, this->EnableAdvancedArrays);
- InitArrayForPolyData(true, internals->ThirdTag, internals->GetTagNames(2), nbrOfPoints, prereservedNbrOfPoints, polyData, this->EnableAdvancedArrays);
+ if (internals->LidarModel != livox::MID_40)
+ {
+ InitArrayForPolyData(true, internals->FirstTag, internals->GetTagName(0), nbrOfPoints, prereservedNbrOfPoints, polyData, this->EnableAdvancedArrays);
+ InitArrayForPolyData(true, internals->SecondTag, internals->GetTagName(1), nbrOfPoints, prereservedNbrOfPoints, polyData, this->EnableAdvancedArrays);
+ }
+ if (internals->LidarModel != livox::MID_40 && internals->LidarModel != livox::HAP)
+ {
+ InitArrayForPolyData(true, internals->ThirdTag, internals->GetTagName(2), nbrOfPoints, prereservedNbrOfPoints, polyData, this->EnableAdvancedArrays);
+ }
// clang-format on
// Set the default array to display in the application
@@ -320,13 +258,16 @@ bool vtkLivoxPacketInterpreter::PreProcessPacket(unsigned char const* data,
unsigned int vtkNotUsed(dataLength),
double& outLidarDataTime)
{
- const SDK2Packet* pkt = reinterpret_cast<const SDK2Packet*>(data);
-
auto& internals = this->Internals;
+ switch (livox::GetSDKType(this->Internals->LidarModel))
+ {
+ case livox::SDK1:
+ return internals->PreProcessPacket<SDK1Packet>(data, this->PublishInterval, outLidarDataTime);
- outLidarDataTime = ::GetEthPacketTimestamp(pkt->timestamp, 8);
-
- return internals->DoSplitPacket(pkt, this->PublishInterval);
+ case livox::SDK2:
+ return internals->PreProcessPacket<SDK2Packet>(data, this->PublishInterval, outLidarDataTime);
+ }
+ return false;
}
//-----------------------------------------------------------------------------
@@ -334,30 +275,32 @@ void vtkLivoxPacketInterpreter::ProcessPacket(unsigned char const* data,
unsigned int vtkNotUsed(dataLength))
{
auto& internals = this->Internals;
- const SDK2Packet* pkt = reinterpret_cast<const SDK2Packet*>(data);
// Split here to not treat twice the same packet
- if (internals->DoSplitPacket(pkt, this->PublishInterval))
- {
- this->SplitFrame();
- }
-
- switch (pkt->dataType)
+ bool doSplit = false;
+ switch (livox::GetSDKType(this->Internals->LidarModel))
{
- case livox::sdk2::CARTESIAN_COORDINATE_HIGH:
- internals->FillHighRawPoints(pkt);
+ case livox::SDK1:
+ doSplit = internals->DoSplitPacket<SDK1Packet>(data, this->PublishInterval);
break;
- case livox::sdk2::CARTESIAN_COORDINATE_LOW:
- internals->FillLowRawPoints(pkt);
+ case livox::SDK2:
+ doSplit = internals->DoSplitPacket<SDK2Packet>(data, this->PublishInterval);
break;
+ }
+ if (doSplit)
+ {
+ Superclass::SplitFrame();
+ }
- case livox::sdk2::SPHERICAL_COORDINATE:
- internals->FillSpherePoints(pkt);
+ switch (livox::GetSDKType(this->Internals->LidarModel))
+ {
+ case livox::SDK1:
+ internals->ProcessSDK1Packet(data);
break;
- default:
- vtkWarningMacro(<< "Could not handle this data type.");
+ case livox::SDK2:
+ internals->ProcessSDK2Packet(data);
break;
}
}
@@ -371,14 +314,11 @@ void vtkLivoxPacketInterpreter::SetLidarModel(int model)
}
auto deviceType = static_cast<livox::DeviceType>(model);
- if (deviceType == livox::MID_360 || deviceType == livox::HAP)
- {
- this->Internals->LidarModel = deviceType;
- this->SetSensorModelName(GetLidarModelName(deviceType));
- this->Modified();
- // Uncomment when changing lidar model change packet format.
- // this->ResetInitializedState();
- }
+ this->Internals->LidarModel = deviceType;
+ this->SetSensorModelName(livox::GetLidarModelName(deviceType));
+ this->Modified();
+
+ this->ResetInitializedState();
}
//-----------------------------------------------------------------------------
diff --git a/LivoxPacketInterpreters/vtkLivoxPacketInterpreter.h b/LivoxPacketInterpreters/vtkLivoxPacketInterpreter.h
index de6e1afd8c984026e84c37f2c8667d2a3e064407..37ef531d7b28ca017d197341ed0ae2c013d784dd 100644
--- a/LivoxPacketInterpreters/vtkLivoxPacketInterpreter.h
+++ b/LivoxPacketInterpreters/vtkLivoxPacketInterpreter.h
@@ -18,8 +18,8 @@
#include <vtkLidarPacketInterpreter.h>
-#include <vtkSetGet.h>
#include <memory>
+#include <vtkSetGet.h>
#include "LivoxPacketInterpretersModule.h"
@@ -38,10 +38,10 @@ public:
bool PreProcessPacket(unsigned char const* data,
unsigned int dataLength,
- double &outLidarDataTime) override;
+ double& outLidarDataTime) override;
vtkGetMacro(PublishInterval, long);
- vtkSetMacro(PublishInterval, long);
+ vtkSetAndResetInitializeMacro(PublishInterval, long);
void SetLidarModel(int type);
int GetLidarModel();
diff --git a/LivoxPosePacketInterpreters/LivoxPoseReader.xml b/LivoxPosePacketInterpreters/LivoxPoseReader.xml
index 1d6316df3c8664079c02db85489db49408aaaf75..dc17a37e128e6ade6f2c81d58f12d71c5779f267 100644
--- a/LivoxPosePacketInterpreters/LivoxPoseReader.xml
+++ b/LivoxPosePacketInterpreters/LivoxPoseReader.xml
@@ -20,6 +20,11 @@
</Documentation>
<Hints>
<PropertyWidgetDecorator type="ConditionalDefaultValueDecorator" property="LidarModel">
+ <Entry default_value="50002" target_value="Mid-40" />
+ <Entry default_value="50002" target_value="Tele-15" />
+ <Entry default_value="50002" target_value="Horizon" />
+ <Entry default_value="50002" target_value="Mid-70" />
+ <Entry default_value="50002" target_value="Avia" />
<Entry default_value="56301" target_value="Mid-360" />
<Entry default_value="57000" target_value="HAP" />
</PropertyWidgetDecorator>
@@ -36,6 +41,11 @@
</Documentation>
<Hints>
<PropertyWidgetDecorator type="ConditionalDefaultValueDecorator" property="LidarModel">
+ <Entry default_value="50002" target_value="Mid-40" />
+ <Entry default_value="50002" target_value="Tele-15" />
+ <Entry default_value="50002" target_value="Horizon" />
+ <Entry default_value="50002" target_value="Mid-70" />
+ <Entry default_value="50002" target_value="Avia" />
<Entry default_value="56401" target_value="Mid-360" />
<Entry default_value="58000" target_value="HAP" />
</PropertyWidgetDecorator>
diff --git a/LivoxPosePacketInterpreters/LivoxPoseStream.xml b/LivoxPosePacketInterpreters/LivoxPoseStream.xml
index b4f5ec0d709fdf607f79f019f92093685cb7fa7f..92a8b6782eca8a98c17fe0a572d439940bb9e984 100644
--- a/LivoxPosePacketInterpreters/LivoxPoseStream.xml
+++ b/LivoxPosePacketInterpreters/LivoxPoseStream.xml
@@ -21,6 +21,11 @@
</Documentation>
<Hints>
<PropertyWidgetDecorator type="ConditionalDefaultValueDecorator" property="LidarModel">
+ <Entry default_value="50002" target_value="Mid-40" />
+ <Entry default_value="50002" target_value="Tele-15" />
+ <Entry default_value="50002" target_value="Horizon" />
+ <Entry default_value="50002" target_value="Mid-70" />
+ <Entry default_value="50002" target_value="Avia" />
<Entry default_value="56301" target_value="Mid-360" />
<Entry default_value="57000" target_value="HAP" />
</PropertyWidgetDecorator>
@@ -39,6 +44,11 @@
</Documentation>
<Hints>
<PropertyWidgetDecorator type="ConditionalDefaultValueDecorator" property="LidarModel">
+ <Entry default_value="50002" target_value="Mid-40" />
+ <Entry default_value="50002" target_value="Tele-15" />
+ <Entry default_value="50002" target_value="Horizon" />
+ <Entry default_value="50002" target_value="Mid-70" />
+ <Entry default_value="50002" target_value="Avia" />
<Entry default_value="56401" target_value="Mid-360" />
<Entry default_value="58000" target_value="HAP" />
</PropertyWidgetDecorator>
diff --git a/LivoxPosePacketInterpreters/vtkLivoxPosePacketInterpreter.cxx b/LivoxPosePacketInterpreters/vtkLivoxPosePacketInterpreter.cxx
index 6cad05e67d2b1767c84253ed70fc324d5bd82b5f..8a18bc69a552aba920faa85023b8e23a8c80b421 100644
--- a/LivoxPosePacketInterpreters/vtkLivoxPosePacketInterpreter.cxx
+++ b/LivoxPosePacketInterpreters/vtkLivoxPosePacketInterpreter.cxx
@@ -24,27 +24,47 @@
namespace
{
+typedef livox::sdk1::EthernetPacket SDK1Packet;
typedef livox::sdk2::EthernetPacket SDK2Packet;
-
-typedef union
-{
- uint8_t stamp_bytes[8];
- int64_t stamp;
-} LdsStamp;
-
-uint64_t GetEthPacketTimestamp(const uint8_t* time_stamp, uint8_t size)
-{
- LdsStamp time;
- memcpy(time.stamp_bytes, time_stamp, size);
- return time.stamp;
-}
-
-const uint64_t kRatioOfMsToNs = 1000000;
}
//-----------------------------------------------------------------------------
class vtkLivoxPosePacketInterpreter::vtkInternals
{
+public:
+ //-----------------------------------------------------------------------------
+ void InsertIMUData(const uint8_t* data, uint32_t idx, double ptsTimestamp)
+ {
+ const auto* imuData = reinterpret_cast<const livox::ImuRawPoint*>(data);
+ this->GyroX->InsertNextValue(imuData[idx].gyroX);
+ this->GyroY->InsertNextValue(imuData[idx].gyroY);
+ this->GyroZ->InsertNextValue(imuData[idx].gyroZ);
+ this->AccelX->InsertNextValue(imuData[idx].accX);
+ this->AccelY->InsertNextValue(imuData[idx].accY);
+ this->AccelZ->InsertNextValue(imuData[idx].accZ);
+ this->Timestamp->InsertNextValue(ptsTimestamp);
+ }
+
+ //-----------------------------------------------------------------------------
+ void ProcessSDK1Packet(unsigned char const* data)
+ {
+ const SDK1Packet* pkt = reinterpret_cast<const SDK1Packet*>(data);
+ uint64_t ptsTimestamp = livox::GetEthPacketTimestamp(pkt->timestamp, 8);
+ this->InsertIMUData(pkt->data, 0, ptsTimestamp);
+ }
+
+ //-----------------------------------------------------------------------------
+ void ProcessSDK2Packet(unsigned char const* data)
+ {
+ const SDK2Packet* pkt = reinterpret_cast<const SDK2Packet*>(data);
+ for (uint32_t idx = 0; idx < pkt->dotNum; idx++)
+ {
+ uint64_t ptsTimestamp =
+ livox::GetEthPacketTimestamp(pkt->timestamp, 8) + idx * pkt->timeInterval;
+ this->InsertIMUData(pkt->data, idx, ptsTimestamp);
+ }
+ }
+
public:
vtkSmartPointer<vtkFloatArray> GyroX;
vtkSmartPointer<vtkFloatArray> GyroY;
@@ -81,41 +101,29 @@ void vtkLivoxPosePacketInterpreter::InitArrays()
}
//----------------------------------------------------------------------------
-bool vtkLivoxPosePacketInterpreter::IsValidPacket(unsigned char const* packetData,
+bool vtkLivoxPosePacketInterpreter::IsValidPacket(unsigned char const* data,
unsigned int dataLength)
{
- const SDK2Packet* dataPacket = reinterpret_cast<const SDK2Packet*>(packetData);
-
- if (!dataPacket)
- {
- return false;
- }
-
- if (dataPacket->length != dataLength)
- {
- return false;
- }
- return dataPacket->dataType == livox::sdk2::IMU_DATA;
+ // We cannot determine which sdk is currently use - must test for both
+ int ret = 0;
+ ret += livox::IsValidPacket(reinterpret_cast<const SDK1Packet*>(data), dataLength, true);
+ ret += livox::IsValidPacket(reinterpret_cast<const SDK2Packet*>(data), dataLength, true);
+ return !!ret;
}
//----------------------------------------------------------------------------
-void vtkLivoxPosePacketInterpreter::ProcessPacket(unsigned char const* packetData,
- unsigned int vtkNotUsed(dataLength))
+void vtkLivoxPosePacketInterpreter::ProcessPacket(unsigned char const* data,
+ unsigned int dataLength)
{
- const SDK2Packet* pkt = reinterpret_cast<const SDK2Packet*>(packetData);
+ auto& internal = this->Internals;
- const auto* data = reinterpret_cast<const livox::ImuRawPoint*>(pkt->data);
- auto& internals = this->Internals;
- for (uint32_t i = 0; i < pkt->dotNum; i++)
+ if (livox::IsValidPacket(reinterpret_cast<const SDK1Packet*>(data), dataLength, true))
+ {
+ internal->ProcessSDK1Packet(data);
+ }
+ else if (livox::IsValidPacket(reinterpret_cast<const SDK2Packet*>(data), dataLength, true))
{
- uint64_t ptsTimestamp = GetEthPacketTimestamp(pkt->timestamp, 8) + i * pkt->timeInterval;
- internals->GyroX->InsertNextValue(data[i].gyroX);
- internals->GyroY->InsertNextValue(data[i].gyroY);
- internals->GyroZ->InsertNextValue(data[i].gyroZ);
- internals->AccelX->InsertNextValue(data[i].accX);
- internals->AccelY->InsertNextValue(data[i].accY);
- internals->AccelZ->InsertNextValue(data[i].accZ);
- internals->Timestamp->InsertNextValue(ptsTimestamp);
+ internal->ProcessSDK2Packet(data);
}
}