From 6f9670bbb3d235d98de93d7c964b7486a39fe50a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Timoth=C3=A9e=20Couble?= Date: Mon, 20 Jan 2025 09:27:52 +0100 Subject: [PATCH 1/6] [feat] Add option to inverse X and Y axis --- LeishenPacketInterpreters/LeishenLidarReader.xml | 1 + LeishenPacketInterpreters/LeishenLidarStream.xml | 1 + .../LeishenPacketInterpreter.xml | 13 +++++++++++++ .../vtkLeishenPacketInterpreter.cxx | 12 ++++++++++-- .../vtkLeishenPacketInterpreter.h | 9 +++++++++ 5 files changed, 34 insertions(+), 2 deletions(-) diff --git a/LeishenPacketInterpreters/LeishenLidarReader.xml b/LeishenPacketInterpreters/LeishenLidarReader.xml index 3baf995..73758d0 100644 --- a/LeishenPacketInterpreters/LeishenLidarReader.xml +++ b/LeishenPacketInterpreters/LeishenLidarReader.xml @@ -38,6 +38,7 @@ + diff --git a/LeishenPacketInterpreters/LeishenLidarStream.xml b/LeishenPacketInterpreters/LeishenLidarStream.xml index 6d966ba..0826abf 100644 --- a/LeishenPacketInterpreters/LeishenLidarStream.xml +++ b/LeishenPacketInterpreters/LeishenLidarStream.xml @@ -28,6 +28,7 @@ + diff --git a/LeishenPacketInterpreters/LeishenPacketInterpreter.xml b/LeishenPacketInterpreters/LeishenPacketInterpreter.xml index 879343b..a4cbfc8 100644 --- a/LeishenPacketInterpreters/LeishenPacketInterpreter.xml +++ b/LeishenPacketInterpreters/LeishenPacketInterpreter.xml @@ -6,6 +6,19 @@ base_proxygroup="internal_interpreter" base_proxyname="CommonLidarPacketInterpreter"> + + + + Check this option to inverse X and Y axis. + + + InverseXY) + { + pos[0] = distance * std::cos(verticalAngle) * std::cos(horizontalAngle); + pos[1] = distance * std::cos(verticalAngle) * std::sin(horizontalAngle); + } + else + { + pos[0] = distance * std::cos(verticalAngle) * std::sin(horizontalAngle); + pos[1] = distance * std::cos(verticalAngle) * std::cos(horizontalAngle); + } pos[2] = distance * std::sin(verticalAngle); if (pos[0] == 0. && pos[1] == 0. && pos[2] == 0.) diff --git a/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.h b/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.h index 260695a..1920dc8 100644 --- a/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.h +++ b/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.h @@ -71,6 +71,14 @@ public: vtkGetMacro(SkipPointMultipleReturn, bool); ///@} + ///@{ + /** + * Leishen ROS driver have this option which is reflected here. + */ + vtkSetMacro(InverseXY, bool); + vtkGetMacro(InverseXY, bool); + ///@} + protected: /** * Creates a new empty frame object, which will be filled by ProcessPacket. @@ -86,6 +94,7 @@ private: void operator=(const vtkLeishenPacketInterpreter&) = delete; bool SkipPointMultipleReturn = false; + bool InverseXY = false; class vtkInternals; std::unique_ptr Internals; -- GitLab From dfa9fd47d3f8db10d88a2e754337ac8a0a527d9e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Timoth=C3=A9e=20Couble?= Date: Mon, 20 Jan 2025 09:29:15 +0100 Subject: [PATCH 2/6] [fix] Ensure leishen data structure are packed --- LeishenPacketInterpreters/CXPacketFormat.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/LeishenPacketInterpreters/CXPacketFormat.h b/LeishenPacketInterpreters/CXPacketFormat.h index 0c8e7cc..6cd7437 100644 --- a/LeishenPacketInterpreters/CXPacketFormat.h +++ b/LeishenPacketInterpreters/CXPacketFormat.h @@ -51,6 +51,9 @@ constexpr double C16_BLOCK_TIME_INTERVAL = 50.; // in microseconds constexpr double C32_CHANNEL_TIME_INTERVAL = 1.536; // in microseconds constexpr double C32_BLOCK_TIME_INTERVAL = 49.152; // in microseconds +// Instructs the compiler to pack structure members +#pragma pack(push, 1) + //----------------------------------------------------------------------------- enum class EchoMode { @@ -237,6 +240,9 @@ public: uint16_t GetCX32HorizontalOffset(uint8_t spot) const { return this->CX32HorizontalOffset[spot]; }; int16_t GetCX16VerticalAngle(uint8_t idx) const { return this->CX16VerticalAngle[idx]; }; }; + +#pragma pack(pop) + }; #endif // CXPacketFormat_h -- GitLab From c15ea3ab1f329165d5edf46327a51929f84a8aa3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Timoth=C3=A9e=20Couble?= Date: Mon, 20 Jan 2025 09:33:20 +0100 Subject: [PATCH 3/6] [feat] Change default meters unit for MS LiDARs --- LeishenPacketInterpreters/CXPacketFormat.h | 1 + LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/LeishenPacketInterpreters/CXPacketFormat.h b/LeishenPacketInterpreters/CXPacketFormat.h index 6cd7437..60d28fc 100644 --- a/LeishenPacketInterpreters/CXPacketFormat.h +++ b/LeishenPacketInterpreters/CXPacketFormat.h @@ -46,6 +46,7 @@ constexpr int16_t C32_VERTICAL_ANGLES[leishen_cx::C32_SIZE] = // clang-format on constexpr double DISTANCE_UNIT = 0.0025; // in meters +constexpr double MS_DISTANCE_UNIT = 0.004; // in meters constexpr double C16_CHANNEL_TIME_INTERVAL = 3.125; // in microseconds constexpr double C16_BLOCK_TIME_INTERVAL = 50.; // in microseconds constexpr double C32_CHANNEL_TIME_INTERVAL = 1.536; // in microseconds diff --git a/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx b/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx index d07804d..ae0d53d 100644 --- a/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx +++ b/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx @@ -232,7 +232,9 @@ void vtkLeishenPacketInterpreter::ProcessPacket(unsigned char const* data, unsig double verticalAngle = isC16 ? internals->C16VerticalAngles[laserId] : leishen_cx::C32_VERTICAL_ANGLES[laserId]; - double distance = currentBlock.Channels[channelId].GetDistance() * leishen_cx::DISTANCE_UNIT; + const double distanceUnit = + internals->IsMSCModel ? leishen_cx::MS_DISTANCE_UNIT : leishen_cx::DISTANCE_UNIT; + double distance = currentBlock.Channels[channelId].GetDistance() * distanceUnit; double intensity = currentBlock.Channels[channelId].GetReflectivity(); if (distance <= 0.1 || distance > 500.0) -- GitLab From 9d3ea18203819e41efd8428666f43647e5faeda9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Timoth=C3=A9e=20Couble?= Date: Mon, 20 Jan 2025 12:12:35 +0100 Subject: [PATCH 4/6] [fix] Correct vertical angles for MS/C16 --- LeishenPacketInterpreters/CXPacketFormat.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LeishenPacketInterpreters/CXPacketFormat.h b/LeishenPacketInterpreters/CXPacketFormat.h index 60d28fc..aa73e41 100644 --- a/LeishenPacketInterpreters/CXPacketFormat.h +++ b/LeishenPacketInterpreters/CXPacketFormat.h @@ -39,7 +39,7 @@ constexpr uint8_t C16_SIZE = 16; constexpr uint8_t C32_SIZE = 32; // clang-format off constexpr std::array C16_VERTICAL_ANGLES = - { -15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15 }; + {-16, 0, -14, 2, -12, 4, -10, 6, -8, 8, -6, 10, -4, 12, -2, 14}; constexpr int16_t C32_VERTICAL_ANGLES[leishen_cx::C32_SIZE] = { -16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7, -8, 8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15}; -- GitLab From 307764ba04ea76be3925b65f73ce47333eafdb60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Timoth=C3=A9e=20Couble?= Date: Mon, 20 Jan 2025 16:21:53 +0100 Subject: [PATCH 5/6] [fix] Correct laser offset --- LeishenPacketInterpreters/CXPacketFormat.h | 2 ++ .../vtkLeishenPacketInterpreter.cxx | 16 ++++++++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/LeishenPacketInterpreters/CXPacketFormat.h b/LeishenPacketInterpreters/CXPacketFormat.h index aa73e41..728c7b2 100644 --- a/LeishenPacketInterpreters/CXPacketFormat.h +++ b/LeishenPacketInterpreters/CXPacketFormat.h @@ -45,6 +45,8 @@ constexpr int16_t C32_VERTICAL_ANGLES[leishen_cx::C32_SIZE] = -8, 8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15}; // clang-format on +constexpr double R1_FACTOR = 0.0361; // in meters +constexpr double CONVERSION_ANGLE = 20.25; // in degrees constexpr double DISTANCE_UNIT = 0.0025; // in meters constexpr double MS_DISTANCE_UNIT = 0.004; // in meters constexpr double C16_CHANNEL_TIME_INTERVAL = 3.125; // in microseconds diff --git a/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx b/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx index ae0d53d..0197ff6 100644 --- a/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx +++ b/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.cxx @@ -248,13 +248,21 @@ void vtkLeishenPacketInterpreter::ProcessPacket(unsigned char const* data, unsig verticalAngle = vtkMath::RadiansFromDegrees(verticalAngle); if (this->InverseXY) { - pos[0] = distance * std::cos(verticalAngle) * std::cos(horizontalAngle); - pos[1] = distance * std::cos(verticalAngle) * std::sin(horizontalAngle); + double laserCorrection = + vtkMath::RadiansFromDegrees(leishen_cx::CONVERSION_ANGLE - currentAzimuth * 0.01); + pos[0] = distance * std::cos(verticalAngle) * std::cos(horizontalAngle) + + leishen_cx::R1_FACTOR * std::cos(laserCorrection); + pos[1] = distance * std::cos(verticalAngle) * std::sin(horizontalAngle) + + leishen_cx::R1_FACTOR * std::sin(laserCorrection); } else { - pos[0] = distance * std::cos(verticalAngle) * std::sin(horizontalAngle); - pos[1] = distance * std::cos(verticalAngle) * std::cos(horizontalAngle); + double laserCorrection = + vtkMath::RadiansFromDegrees(currentAzimuth * 0.01 - leishen_cx::CONVERSION_ANGLE); + pos[0] = distance * std::cos(verticalAngle) * std::sin(horizontalAngle) + + leishen_cx::R1_FACTOR * std::sin(laserCorrection); + pos[1] = distance * std::cos(verticalAngle) * std::cos(horizontalAngle) + + leishen_cx::R1_FACTOR * std::cos(laserCorrection); } pos[2] = distance * std::sin(verticalAngle); -- GitLab From 2cb57342058f32201a4c75fb18d11366541efc68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Timoth=C3=A9e=20Couble?= Date: Mon, 20 Jan 2025 17:00:00 +0100 Subject: [PATCH 6/6] [refact] Change test baseline --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 435d9f1..461fa50 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ if (BUILD_TESTING) include(FetchTestData) lidarview_fetch_test_data( DATA_NAME "leishen" - COMMIT_SHA "b885f7e23304aaadeae0272b25797b0b9f47c8c1" + COMMIT_SHA "5e72313a301381d91dd667ed3d7a7b4955518feb" ) set(leishen_test_data_dir "${FETCH_TEST_DATA_DIR}") endif () -- GitLab