From c6b9f8ab6cab4fc820c84602b421e65734fa0266 Mon Sep 17 00:00:00 2001 From: Michael Lehning Date: Thu, 2 Jul 2020 16:57:38 +0200 Subject: [PATCH] angle compensation of NAV3xx optimized --- CMakeLists.txt | 66 +++++++------------- driver/src/dataDumper.cpp | 2 + driver/src/helper/angle_compensator.cpp | 36 +++++++++++ driver/src/sick_generic_caller.cpp | 5 +- driver/src/sick_generic_parser.cpp | 40 ++++++------ driver/src/sick_scan_common.cpp | 30 ++++----- driver/src/sick_scan_common_tcp.cpp | 4 +- include/sick_scan/helper/angle_compensator.h | 1 + include/sick_scan/sick_generic_parser.h | 6 +- package.xml | 10 +-- 10 files changed, 109 insertions(+), 91 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1267249..bdcccb3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,9 +23,12 @@ endmacro(use_cxx11) # Switch on, if you use c11-specific commands use_cxx11() +set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-format-overflow") # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wpedantic") +# + project(sick_scan) @@ -48,10 +51,12 @@ project(sick_scan) # set(CATKIN_DEVEL_PREFIX "${CMAKE_BINARY_DIR}/devel") # set(CMAKE_PREFIX_PATH "${CMAKE_BINARY_DIR}/devel;/opt/ros/melodic") +find_package(Boost REQUIRED COMPONENTS system) + -find_package(catkin REQUIRED - COMPONENTS +find_package(catkin REQUIRED COMPONENTS roscpp + roslib # needed ros::package::getPath() sensor_msgs diagnostic_updater dynamic_reconfigure @@ -60,14 +65,12 @@ find_package(catkin REQUIRED sensor_msgs visualization_msgs message_generation - pcl_conversions - pcl_ros ) find_package(PkgConfig REQUIRED) -find_package(Boost REQUIRED COMPONENTS system) + generate_dynamic_reconfigure_options( cfg/SickScan.cfg @@ -96,12 +99,13 @@ generate_messages( ) catkin_package( - DEPENDS Boost - CATKIN_DEPENDS message_runtime roscpp sensor_msgs diagnostic_updater dynamic_reconfigure + CATKIN_DEPENDS message_runtime roscpp sensor_msgs diagnostic_updater dynamic_reconfigure perception_pcl LIBRARIES sick_scan_lib - INCLUDE_DIRS include) + INCLUDE_DIRS include + DEPENDS Boost +) -include_directories(include include/tinyxml ${catkin_INCLUDE_DIRS} include/sick_scan) +include_directories(include include/tinyxml ${catkin_INCLUDE_DIRS} include/sick_scan) add_library(sick_scan_lib driver/src/dataDumper.cpp @@ -145,22 +149,19 @@ add_executable(radar_object_marker tools/radar_object_marker/src/radar_object_marker.cpp tools/pcl_converter/src/gnuplotPaletteReader.cpp include/radar_object_marker/radar_object_marker.h) -target_link_libraries(radar_object_marker sick_scan_lib) +target_link_libraries(radar_object_marker sick_scan_lib ) # # # -add_executable(pcl_converter tools/pcl_converter/src/pcl_converter.cpp tools/pcl_converter/src/gnuplotPaletteReader.cpp) - -target_link_libraries(pcl_converter - ${catkin_LIBRARIES} - ${roslib_LIBRARIES} - ${PCL_LIBRARIES}) - - -# add_executable( pcl_find_plane tools/pcl_converter/src/pcl_find_plane.cpp) -# target_link_libraries(pcl_find_plane ${catkin_LIBRARIES}) +# +# pcl_converter disabled to avoid dependency to pcl +# +# add_executable(pcl_converter tools/pcl_converter/src/pcl_converter.cpp tools/pcl_converter/src/gnuplotPaletteReader.cpp) +# +# target_link_libraries(pcl_converter +# ${catkin_LIBRARIES}) target_link_libraries(sick_generic_caller sick_scan_lib) @@ -177,31 +178,6 @@ target_link_libraries(sick_scan_test ${roslib_LIBRARIES} sick_scan_lib) -# -# Sensor alignment tool -# -# currently disabled for trusty support -# add_executable(sensor_alignment -# tools/sensor_alignment/sensor_alignment_tf_dyn.cpp -# ) -# -# target_link_libraries(sensor_alignment -# ${catkin_LIBRARIES} -# ) -# add_dependencies(sensor_alignment ${PROJECT_NAME}_gencfg) - -# target_link_libraries(${PROJECT_NAME}_pointcloud2_refresh -# ${catkin_LIBRARIES} -# ) - - -# Destinations: -# LIB for library -# BIN for Binaries -# INCLUDE for Includes -# SHARE for config files, launch files etc. - - install(TARGETS sick_scan_lib DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/driver/src/dataDumper.cpp b/driver/src/dataDumper.cpp index 35a0e18..abc7777 100644 --- a/driver/src/dataDumper.cpp +++ b/driver/src/dataDumper.cpp @@ -64,6 +64,7 @@ int DataDumper::writeToFileNameWhenBufferIsFull(std::string filename) int DataDumper::dumpUcharBufferToConsole(unsigned char *buffer, int bufLen) { + int ret = 0; char asciiBuffer[255] = {0}; for (int i = 0; i < bufLen; i++) { @@ -94,6 +95,7 @@ int DataDumper::dumpUcharBufferToConsole(unsigned char *buffer, int bufLen) printf("%s\n", asciiBuffer); } + return(ret); } int DataDumper::testbed() diff --git a/driver/src/helper/angle_compensator.cpp b/driver/src/helper/angle_compensator.cpp index f39ed20..71e05a0 100644 --- a/driver/src/helper/angle_compensator.cpp +++ b/driver/src/helper/angle_compensator.cpp @@ -97,6 +97,40 @@ using namespace std; */ +/*! +\brief Compensate raw angle given in [RAD] in the ROS axis orientation system +\param angleInRad: raw angle in [RAD] ( +*/ +double AngleCompensator::compensateAngleInRadFromRos(double angleInRadFromRos) +{ + // this is a NAV3xx - X-Axis is the same like ROS + // but we rotate clockwise instead of counter clockwise + double angleInRadFromSickOrg = 0.0; + double angleInRadToRosCompensated = 0.0; + if (useNegSign) + { + angleInRadFromSickOrg = -angleInRadFromRos; + } + else // NAV2xx + { + // NAV2xx uses "standard" counter clockwise rotation like ROS, but X-Axis shows to the right + angleInRadFromSickOrg = angleInRadFromRos + M_PI/2.0; + } + + double angleInRadFromSickCompensated = compensateAngleInRad(angleInRadFromSickOrg); + + if (useNegSign) // NAV3xx + { + angleInRadToRosCompensated = -angleInRadFromSickCompensated; + } + else // NAV2xx + { + // NAV2xx uses "standard" counter clockwise rotation like ROS + angleInRadToRosCompensated = angleInRadFromSickCompensated - M_PI/2.0; + } + return(angleInRadToRosCompensated); +} + /*! \brief Compensate raw angle given in [RAD] \param angleInRad: raw angle in [RAD] @@ -109,6 +143,7 @@ double AngleCompensator::compensateAngleInRad(double angleInRad) { sign = -1; } + //angleInRad *= sign; double angleCompInRad = angleInRad + deg2radFactor * amplCorr * sin(angleInRad + sign * phaseCorrInRad) + offsetCorrInRad; return(angleCompInRad); } @@ -124,6 +159,7 @@ double AngleCompensator::compensateAngleInDeg(double angleInDeg) { sign = -1; } + //angleInDeg*=sign; // AngleComp =AngleRaw + AngleCompAmpl * SIN( AngleRaw + AngleCompPhase ) + AngleCompOffset double angleCompInDeg; double deg2radFactor = 0.01745329252; // pi/180 deg - see for example: https://www.rapidtables.com/convert/number/degrees-to-radians.html diff --git a/driver/src/sick_generic_caller.cpp b/driver/src/sick_generic_caller.cpp index 57fd4d4..fd30fad 100644 --- a/driver/src/sick_generic_caller.cpp +++ b/driver/src/sick_generic_caller.cpp @@ -120,10 +120,13 @@ // 1.7.2: 2020-06-09: TiM433 added and launch file info for TiM4xx added // 1.7.3: 2020-06-10: NAV 3xx angle correction added // 1.7.4: 2020-06-10: NAV 3xx angle correction improved +// 1.7.5: 2020-06-25: Preparing for Release Noetic +// 1.7.6: 2020-02-07: NAV310 handling optimized + #define SICK_GENERIC_MAJOR_VER "1" #define SICK_GENERIC_MINOR_VER "7" -#define SICK_GENERIC_PATCH_LEVEL "4" +#define SICK_GENERIC_PATCH_LEVEL "6" #include // for std::min diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp index 0c2d8d8..bd96a64 100644 --- a/driver/src/sick_generic_parser.cpp +++ b/driver/src/sick_generic_parser.cpp @@ -267,9 +267,9 @@ namespace sick_scan \param _scanMirrored: false for normal mounting true for up side down or NAV 310 \sa setScanMirrored */ - void ScannerBasicParam::setScanMirrored(bool _scannMirrored) + void ScannerBasicParam::setScanMirroredAndShifted(bool _scannMirroredAndShifted) { - scanMirrored = _scannMirrored; + scanMirroredAndShifted = _scannMirroredAndShifted; } /*! @@ -277,9 +277,9 @@ namespace sick_scan \param _scanMirrored: false for normal mounting true for up side down or NAV 310 \sa getScanMirrored */ - bool ScannerBasicParam::getScanMirrored(void) + bool ScannerBasicParam::getScanMirroredAndShifted(void) { - return (scanMirrored); + return (scanMirroredAndShifted); } /*! @@ -409,7 +409,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0) // LMS1000 - 4 layer, 1101 shots per scan @@ -424,7 +424,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_240_NAME) == 0) // TIM_5xx - 1 Layer, max. 811 shots per scan @@ -438,7 +438,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_5XX_NAME) == @@ -453,7 +453,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_4XXX_NAME) == 0) // LMS_4xxx - 1 Layer, 600 Hz @@ -467,7 +467,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XX_NAME) == 0) // TIM_7xx - 1 Layer Scanner { @@ -480,7 +480,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) // TIM_7xxS - 1 layer Safety Scanner { @@ -493,7 +493,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(true); // Safety scanner basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) // LMS_5xx - 1 Layer { @@ -506,7 +506,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XX_NAME) == 0) // LMS_1xx - 1 Layer { @@ -519,7 +519,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_MRS_6XXX_NAME) == 0) // { @@ -533,7 +533,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_3XX_NAME) == 0) // Radar @@ -548,7 +548,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(true); // Device is a radar basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // Nav 3xx { @@ -559,7 +559,7 @@ namespace sick_scan basicParams[i].setExpectedFrequency(55.0); basicParams[i].setUseBinaryProtocol(true); basicParams[i].setDeviceIsRadar(false); // Default - basicParams[i].setScanMirrored(true); // other ortation direction than other scanners + basicParams[i].setScanMirroredAndShifted(true); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0) // NAV_2xx - 1 Layer { @@ -572,7 +572,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_4XX_NAME) == 0) // TiM433 and TiM443 { @@ -585,7 +585,7 @@ namespace sick_scan basicParams[i].setDeviceIsRadar(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setScanMirrored(false); + basicParams[i].setScanMirroredAndShifted(false); } } @@ -801,7 +801,7 @@ namespace sick_scan if (verboseLevel > 0) { static int cnt = 0; - char szDumpFileName[255] = {0}; + char szDumpFileName[511] = {0}; char szDir[255] = {0}; #ifdef _MSC_VER strcpy(szDir,"C:\\temp\\"); @@ -845,7 +845,7 @@ namespace sick_scan if (verboseLevel > 0) { static int cnt = 0; - char szDumpFileName[255] = {0}; + char szDumpFileName[511] = {0}; char szDir[255] = {0}; #ifdef _MSC_VER strcpy(szDir,"C:\\temp\\"); diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp index 8156f70..875f652 100644 --- a/driver/src/sick_scan_common.cpp +++ b/driver/src/sick_scan_common.cpp @@ -1146,7 +1146,7 @@ namespace sick_scan { boost::system::error_code ec; ipNewIPAddr = boost::asio::ip::address_v4::from_string(sNewIPAddr, ec); - if (ec == 0) + if (ec == boost::system::errc::success) { sopasCmdChain.clear(); sopasCmdChain.push_back(CMD_SET_ACCESS_MODE_3); @@ -1166,7 +1166,7 @@ namespace sick_scan { boost::system::error_code ec; NTPIpAdress = boost::asio::ip::address_v4::from_string(sNTPIpAdress, ec); - if (ec != 0) + if (ec != boost::system::errc::success) { setUseNTP = false; ROS_ERROR("ERROR: NTP Server IP ADDRESS could not be parsed Boost Error %s:%d", ec.category().name(), @@ -2530,7 +2530,7 @@ namespace sick_scan { bool ret = true; static int cnt = 0; - char szDumpFileName[255] = {0}; + char szDumpFileName[511] = {0}; char szDir[255] = {0}; if (cnt == 0) { @@ -3174,16 +3174,18 @@ namespace sick_scan msg.angle_increment = sizeOfSingleAngularStep; msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment; - if (this->parser_->getCurrentParamPtr()->getScanMirrored()) + if (this->parser_->getCurrentParamPtr()->getScanMirroredAndShifted()) { msg.angle_min *= -1.0; msg.angle_increment *= -1.0; msg.angle_max *= -1.0; - double tmp; tmp = msg.angle_min; msg.angle_min = msg.angle_max; - msg.angle_max = msg.angle_min; + msg.angle_max = tmp; + + msg.angle_min += M_PI/2.0; + msg.angle_max += M_PI/2.0; } float *rangePtr = NULL; @@ -3567,15 +3569,17 @@ namespace sick_scan // prepare lookup for elevation angle table - std::vector cosAlphaTable; - std::vector sinAlphaTable; + std::vector cosAlphaTable; // Lookup table for cos + std::vector sinAlphaTable; // Lookup table for sin int rangeNum = rangeTmp.size() / numValidEchos; cosAlphaTable.resize(rangeNum); sinAlphaTable.resize(rangeNum); float mirror_factor = 1.0; - if (this->parser_->getCurrentParamPtr()->getScanMirrored()) + float angleShift=0; + if (this->parser_->getCurrentParamPtr()->getScanMirroredAndShifted()) { mirror_factor = -1.0; + angleShift = -M_PI/2.0; // subtract 90 deg for NAV3xx-series } for (size_t iEcho = 0; iEcho < numValidEchos; iEcho++) @@ -3631,7 +3635,7 @@ namespace sick_scan if (iEcho == 0) { - cosAlphaTablePtr[i] = cos(alpha); + cosAlphaTablePtr[i] = cos(alpha); // for z-value (elevation) sinAlphaTablePtr[i] = sin(alpha); } else @@ -3641,12 +3645,10 @@ namespace sick_scan // Thanks to Sebastian Pütz for his hint float rangeCos = range_meter * cosAlphaTablePtr[i]; - double phi_used = phi * mirror_factor; + double phi_used = phi + angleShift; if (this->angleCompensator != NULL) { - phi_used += M_PI/2.0; // 90° SICK corresponds to 0° ROS. We muss add 90° before using the lookup table - phi_used = angleCompensator->compensateAngleInRad(phi_used); - phi_used -= M_PI/2.0; + phi_used = angleCompensator->compensateAngleInRadFromRos(phi_used); } fptr[idx_x] = rangeCos * cos(phi_used); // copy x value in pointcloud fptr[idx_y] = rangeCos * sin(phi_used); // copy y value in pointcloud diff --git a/driver/src/sick_scan_common_tcp.cpp b/driver/src/sick_scan_common_tcp.cpp index b2b7143..3cc1e3f 100644 --- a/driver/src/sick_scan_common_tcp.cpp +++ b/driver/src/sick_scan_common_tcp.cpp @@ -616,7 +616,9 @@ namespace sick_scan int SickScanCommonTcp::numberOfDatagramInInputFifo() { - this->recvQueue.getNumberOfEntriesInQueue(); + int ret = 0; + ret = this->recvQueue.getNumberOfEntriesInQueue(); + return(ret); } int SickScanCommonTcp::readWithTimeout(size_t timeout_ms, char *buffer, int buffer_size, int *bytes_read, diff --git a/include/sick_scan/helper/angle_compensator.h b/include/sick_scan/helper/angle_compensator.h index decc398..e4b7d2f 100644 --- a/include/sick_scan/helper/angle_compensator.h +++ b/include/sick_scan/helper/angle_compensator.h @@ -11,6 +11,7 @@ class AngleCompensator { public: + double compensateAngleInRadFromRos(double angleInRadFromRos); double compensateAngleInRad(double angleInRad); double compensateAngleInDeg(double angleInDeg); int parseAsciiReply(const char *asciiReply); diff --git a/include/sick_scan/sick_generic_parser.h b/include/sick_scan/sick_generic_parser.h index f671f71..d176043 100644 --- a/include/sick_scan/sick_generic_parser.h +++ b/include/sick_scan/sick_generic_parser.h @@ -91,9 +91,9 @@ namespace sick_scan bool getUseBinaryProtocol(void); - void setScanMirrored(bool _scanMirrored); + void setScanMirroredAndShifted(bool _scanMirroredAndShifted); - bool getScanMirrored(); + bool getScanMirroredAndShifted(); void setUseBinaryProtocol(bool _useBinary); @@ -129,7 +129,7 @@ namespace sick_scan bool useSafetyPasWD; int8_t encoderMode; bool CartographerCompatibility; - bool scanMirrored; + bool scanMirroredAndShifted; }; diff --git a/package.xml b/package.xml index ca4bcd6..5ab2807 100644 --- a/package.xml +++ b/package.xml @@ -1,9 +1,9 @@ sick_scan - 1.7.4 + 1.7.6 - A ROS driver for the SICK TiM and SICK MRS series of laser scanners. + A ROS driver for the SICK TiM and SICK MRS series of lidars. This package is based on the original sick_tim-repository of Martin Günther et al. @@ -24,19 +24,15 @@ diagnostic_updater dynamic_reconfigure message_generation - pcl_conversions - pcl_ros roscpp + perception_pcl sensor_msgs diagnostic_updater dynamic_reconfigure message_runtime - pcl_conversions - pcl_ros visualization_msgs -