diff --git a/CMakeLists.txt b/CMakeLists.txt index 435d9f1a425d727c0e507e77b7676a0cd8f1988b..461fa50bee01d8bcb54f12821ffc349a11eedc84 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 () diff --git a/LeishenPacketInterpreters/CXPacketFormat.h b/LeishenPacketInterpreters/CXPacketFormat.h index 0c8e7cc300677e3b34473d53f8af7e4cc7fae256..728c7b2bf6b7ef29f5859b4364855689922241f2 100644 --- a/LeishenPacketInterpreters/CXPacketFormat.h +++ b/LeishenPacketInterpreters/CXPacketFormat.h @@ -39,18 +39,24 @@ 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}; // 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 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 +243,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 diff --git a/LeishenPacketInterpreters/LeishenLidarReader.xml b/LeishenPacketInterpreters/LeishenLidarReader.xml index 3baf9953aec4c189e6199ae11c4ccfaaa09cf235..73758d03f92ebfbae973f189acf09f74149c01fe 100644 --- a/LeishenPacketInterpreters/LeishenLidarReader.xml +++ b/LeishenPacketInterpreters/LeishenLidarReader.xml @@ -38,6 +38,7 @@ + diff --git a/LeishenPacketInterpreters/LeishenLidarStream.xml b/LeishenPacketInterpreters/LeishenLidarStream.xml index 6d966ba73288a0e3de3880650cd2d24213a217f9..0826abf60fac53cb02e66061d7dc6b451149aa3a 100644 --- a/LeishenPacketInterpreters/LeishenLidarStream.xml +++ b/LeishenPacketInterpreters/LeishenLidarStream.xml @@ -28,6 +28,7 @@ + diff --git a/LeishenPacketInterpreters/LeishenPacketInterpreter.xml b/LeishenPacketInterpreters/LeishenPacketInterpreter.xml index 879343b6850417f364e104bec9080dd9b0b8b4fd..a4cbfc826691976f6e5c6e6da5257eb820db6833 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. + + + 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) @@ -244,8 +246,24 @@ void vtkLeishenPacketInterpreter::ProcessPacket(unsigned char const* data, unsig double pos[3]; horizontalAngle = vtkMath::RadiansFromDegrees(horizontalAngle); verticalAngle = vtkMath::RadiansFromDegrees(verticalAngle); - pos[0] = distance * std::cos(verticalAngle) * std::cos(horizontalAngle); - pos[1] = distance * std::cos(verticalAngle) * std::sin(horizontalAngle); + if (this->InverseXY) + { + 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 + { + 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); if (pos[0] == 0. && pos[1] == 0. && pos[2] == 0.) diff --git a/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.h b/LeishenPacketInterpreters/vtkLeishenPacketInterpreter.h index 260695aed581f10bd0deb0c064d0353701d5916d..1920dc8def9f063ab76d4de8ce1f5cd60a5e3cc9 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;