From b6429909fb711b93f986f4fee9991e9078359bdd Mon Sep 17 00:00:00 2001 From: rostest Date: Thu, 28 Jul 2022 15:02:57 +0200 Subject: [PATCH 1/2] Bugfix MRS6124 pointcloud --- CHANGELOG.md | 3 +++ driver/src/sick_generic_laser.cpp | 2 +- driver/src/sick_generic_parser.cpp | 2 +- driver/src/sick_lmd_scandata_parser.cpp | 4 ++-- include/sick_scan/sick_lmd_scandata_parser.h | 4 ++-- 5 files changed, 9 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8e943c4e..69aad43a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,9 @@ features that will be removed in future versions **Removed** for deprecated feat ## Released ## +### v2.7.5 - + - **Fixed** MRS6124 pointcloud error #88 + ### v2.7.4 - - **Fixed** ROS2 compilation error #83 diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp index 1b28303e..489846fb 100644 --- a/driver/src/sick_generic_laser.cpp +++ b/driver/src/sick_generic_laser.cpp @@ -93,7 +93,7 @@ #define SICK_GENERIC_MAJOR_VER "2" #define SICK_GENERIC_MINOR_VER "7" -#define SICK_GENERIC_PATCH_LEVEL "4" +#define SICK_GENERIC_PATCH_LEVEL "5" #define DELETE_PTR(p) if(p){delete(p);p=0;} diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp index 85e9e43a..3fb60f14 100644 --- a/driver/src/sick_generic_parser.cpp +++ b/driver/src/sick_generic_parser.cpp @@ -771,7 +771,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setImuEnabled(false);// Default - basicParams[i].setScanAngleShift(-M_PI/2); + basicParams[i].setScanAngleShift(-15.0 * M_PI / 180.0); // (-M_PI/2); basicParams[i].setScanMirroredAndShifted(false); basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); basicParams[i].setMaxEvalFields(0); diff --git a/driver/src/sick_lmd_scandata_parser.cpp b/driver/src/sick_lmd_scandata_parser.cpp index 3d899fa3..40665235 100644 --- a/driver/src/sick_lmd_scandata_parser.cpp +++ b/driver/src/sick_lmd_scandata_parser.cpp @@ -80,8 +80,8 @@ namespace sick_scan /** Parse common result telegrams, i.e. parse telegrams of type LMDscandata received from the lidar */ bool parseCommonBinaryResultTelegram(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double& elevationAngleInRad, rosTime& recvTimeStamp, - bool config_sw_pll_only_publish, bool use_generation_timestamp, SickGenericParser* parser_, bool& FireEncoder, sick_scan_msg::Encoder& EncoderMsg, int& numEchos, std::vector vang_vec, - ros_sensor_msgs::LaserScan & msg) + bool config_sw_pll_only_publish, bool use_generation_timestamp, SickGenericParser* parser_, bool& FireEncoder, sick_scan_msg::Encoder& EncoderMsg, int& numEchos, + std::vector& vang_vec, ros_sensor_msgs::LaserScan & msg) { elevAngleX200 = 0; // signed short (F5 B2 -> Layer 24 // F5B2h -> -2638/200= -13.19° diff --git a/include/sick_scan/sick_lmd_scandata_parser.h b/include/sick_scan/sick_lmd_scandata_parser.h index dfac8032..cd5534ab 100644 --- a/include/sick_scan/sick_lmd_scandata_parser.h +++ b/include/sick_scan/sick_lmd_scandata_parser.h @@ -70,8 +70,8 @@ namespace sick_scan /** Parse common result telegrams, i.e. parse telegrams of type LMDscandata received from the lidar */ bool parseCommonBinaryResultTelegram(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double& elevationAngleInRad, rosTime& recvTimeStamp, - bool config_sw_pll_only_publish, bool use_generation_timestamp, SickGenericParser* parser_, bool& FireEncoder, sick_scan_msg::Encoder& EncoderMsg, int& numEchos, - std::vector vang_vec, ros_sensor_msgs::LaserScan & msg); + bool config_sw_pll_only_publish, bool use_generation_timestamp, SickGenericParser* parser_, bool& FireEncoder, sick_scan_msg::Encoder& EncoderMsg, int& numEchos, + std::vector& vang_vec, ros_sensor_msgs::LaserScan & msg); } /* namespace sick_scan */ #endif /* SICK_LMD_SCANDATA_PARSER_H_ */ From ef76ae4032af73934e9f33664423ef45f177b6c2 Mon Sep 17 00:00:00 2001 From: rostest Date: Thu, 28 Jul 2022 19:01:52 +0200 Subject: [PATCH 2/2] angle offset MRS6124 --- driver/src/sick_generic_parser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp index 3fb60f14..85e9e43a 100644 --- a/driver/src/sick_generic_parser.cpp +++ b/driver/src/sick_generic_parser.cpp @@ -771,7 +771,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setImuEnabled(false);// Default - basicParams[i].setScanAngleShift(-15.0 * M_PI / 180.0); // (-M_PI/2); + basicParams[i].setScanAngleShift(-M_PI/2); basicParams[i].setScanMirroredAndShifted(false); basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); basicParams[i].setMaxEvalFields(0);