diff --git a/README.md b/README.md index 03b2332..360e5d2 100644 --- a/README.md +++ b/README.md @@ -200,14 +200,14 @@ The use of the parameters can be looked up in the launch files. This is also rec - `frame_id` Frame id used for the published data -### Further useful parameters +### Further useful parameters and features - `timelimit` Timelimit in [sec] for max. wait time of incoming sensor reply - `sw_pll_only_publish` If true, the internal Software PLL is fored to sync the scan generation time stamp to a system timestamp - +- Angle compensation: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](./doc/angular_compensation.md). ## Sopas Mode This driver supports both COLA-B (binary) and COLA-A (ASCII) communication with the laser scanner. Binary mode is activated by default. Since this mode generates less network traffic. diff --git a/cfg/SickScan.cfg b/cfg/SickScan.cfg index 1c3f9db..43b4985 100755 --- a/cfg/SickScan.cfg +++ b/cfg/SickScan.cfg @@ -53,8 +53,8 @@ from math import pi gen = ParameterGenerator() # Name Type Reconfiguration level Description Default Min Max -gen.add("min_ang", double_t, 0, "The angle of the first range measurement [rad].", -pi,-pi,pi) -gen.add("max_ang", double_t, 0, "The angle of the last range measurement [rad].", pi,-pi,pi) +gen.add("min_ang", double_t, 0, "The angle of the first range measurement [rad].", -pi,-2*pi,2*pi) +gen.add("max_ang", double_t, 0, "The angle of the last range measurement [rad].", pi,-2*pi,2*pi) gen.add("intensity", bool_t, 0, "Whether or not to return intensity values. ", True) gen.add("intensity_resolution_16bit", bool_t, 0, "True = 16Bit False = 8Bit. ", True) gen.add("skip", int_t, 0, "The number of scans to skip between each measured scan.", 0, 0, 9) @@ -73,5 +73,6 @@ gen.add("cloud_output_mode", int_t, 0, "[0] Pointcloud is dense all layers in on gen.add("ang_res", double_t, 0, "Angular resolution in deg/scan set to 0 to use scanner default", 0 ,0,10) gen.add("scan_freq", double_t, 0, "Scan frequency set to 0 to use scanner default",0 ,0,100) gen.add("encoder_mode", int_t, 0, "-1:No Encoder, 0:Off, 1:Single increment, 2:Direction Phase, 3:Direction Level",-1 ,-1,3) +# gen.add("mean_filter", int_t, 0, "Number of averages for mean filter 0 means filter is disabled", 0, 0, 100) # gen.add("mirror_scan",bool_t, 0, "Scan direction's changed. E.g. for overhead mounting or NAV 310 ( in contrast to other sick scanners NAV 310 is clockwise rotating ).",False) exit(gen.generate(PACKAGE, "sick_scan", "SickScan")) diff --git a/doc/angle_compensation/ampl_phase_offset.png b/doc/angle_compensation/ampl_phase_offset.png new file mode 100644 index 0000000..74d6e2d Binary files /dev/null and b/doc/angle_compensation/ampl_phase_offset.png differ diff --git a/doc/faq.md b/doc/faq.md index b2608b4..dbd72d3 100644 --- a/doc/faq.md +++ b/doc/faq.md @@ -1,22 +1,27 @@ # sick_scan FAQ -## rviz shows a grey point cloud +## rviz shows a grey point cloud :question: rviz shows a grey point cloud. The size of points can be adjusted. -:white_check_mark: Check in the launch file that the intensity flag is set to True. +:white_check_mark: Check in the launch file that the intensity flag is set to True. :question: rviz shows a grey point cloud and the size of points can not be adjusted. -:white_check_mark: Probably in this case you are running Linux in a virtual machine. In this case, OpenGL may not work correctly in the VM. rviz then chooses a kind of "fallback solution" and deactivates the colors. +:white_check_mark: Probably in this case you are running Linux in a virtual machine. In this case, OpenGL may not work correctly in the VM. rviz then chooses a kind of "fallback solution" and deactivates the colors. Also, changing the "Size" and "Style" display in rviz has no effect on the display of the pointcloud data. -The problem can be avoided by starting rviz with the following sequence: +The problem can be avoided by starting rviz with the following sequence: ``` -export LIBGL_ALWAYS_SOFTWARE=1 -rosrun rviz rviz +export LIBGL_ALWAYS_SOFTWARE=1 +rosrun rviz rviz ``` -Further information can be found at http://wiki.ros.org/rviz/Troubleshooting. +## Angular resolution and scan frequency +:question: The angular resolution/ number of shots is too small + +:white_check_mark: Possibly Mean or Median filters are activated. Use Sopas ET to deactivate them and store this settings permanent on the device, see picture. +![Sopas_filter](tim5xxx_filter.PNG) +Further information can be found at http://wiki.ros.org/rviz/Troubleshooting. diff --git a/doc/tim5xxx_filter.PNG b/doc/tim5xxx_filter.PNG new file mode 100755 index 0000000..b99685e Binary files /dev/null and b/doc/tim5xxx_filter.PNG differ diff --git a/driver/src/helper/angle_compensator.cpp b/driver/src/helper/angle_compensator.cpp index 7406bce..f39ed20 100644 --- a/driver/src/helper/angle_compensator.cpp +++ b/driver/src/helper/angle_compensator.cpp @@ -161,9 +161,9 @@ int AngleCompensator::parseAsciiReply(const char *replyStr) // 32 // 16 - int16_t ampl10000th; + int32_t ampl10000th; int32_t phase10000th; - int16_t offset10000th; + int32_t offset10000th; if (cont.size() == 5) @@ -181,9 +181,20 @@ int AngleCompensator::parseAsciiReply(const char *replyStr) helperArr[i] = std::stoul(cont[2+i], nullptr, 16); } } - ampl10000th = (int16_t)(0xFFFF & helperArr[0]); - phase10000th = (int32_t)(0xFFFFFFFF & helperArr[1]); // check againt https://www.rapidtables.com/convert/number/hex-to-decimal.html - offset10000th = (int16_t)(0xFFFF & helperArr[2]); + + if(useNegSign) // NAV310 uses 16/32/16 and subtracts given offset from sine-wave-compensation part + { + ampl10000th = (int32_t)((int16_t)(0xFFFF & helperArr[0])); + phase10000th = (int32_t)(0xFFFFFFFF & helperArr[1]); // check againt https://www.rapidtables.com/convert/number/hex-to-decimal.html + offset10000th = (int32_t)(int16_t)(0xFFFF & helperArr[2]); + } + else // NAV210 uses 32/32/32 and POSITIVE Sign for phase offset + { + ampl10000th = (int32_t)(0xFFFFFFFF & helperArr[0]); + phase10000th = (int32_t)(0xFFFFFFFF & helperArr[1]); // check againt https://www.rapidtables.com/convert/number/hex-to-decimal.html + offset10000th = (int32_t)(0xFFFFFFFF & helperArr[2]); + + } } amplCorr = 1/10000.0 * ampl10000th; diff --git a/driver/src/sick_generic_caller.cpp b/driver/src/sick_generic_caller.cpp index b93ef29..57fd4d4 100644 --- a/driver/src/sick_generic_caller.cpp +++ b/driver/src/sick_generic_caller.cpp @@ -119,10 +119,11 @@ // 1.7.1: 2020-06-04: NAV 2xx angle correction added // 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 #define SICK_GENERIC_MAJOR_VER "1" #define SICK_GENERIC_MINOR_VER "7" -#define SICK_GENERIC_PATCH_LEVEL "3" +#define SICK_GENERIC_PATCH_LEVEL "4" #include // for std::min diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp index 51f497d..0c2d8d8 100644 --- a/driver/src/sick_generic_parser.cpp +++ b/driver/src/sick_generic_parser.cpp @@ -748,7 +748,7 @@ namespace sick_scan } float expected_time_increment = - this->getCurrentParamPtr()->getNumberOfLayers() * scan_time * angle_increment / (2.0 * M_PI); + fabs(this->getCurrentParamPtr()->getNumberOfLayers() * scan_time * angle_increment / (2.0 * M_PI));//If the direction of rotation is reversed, i.e. negative angle increment, a negative scan time results. This does not makes sense, therefore the absolute value is calculated. if (fabs(expected_time_increment - time_increment) > 0.00001) { ROS_WARN_THROTTLE(60, diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp index 79d1e92..c02b489 100644 --- a/driver/src/sick_scan_common.cpp +++ b/driver/src/sick_scan_common.cpp @@ -907,6 +907,8 @@ namespace sick_scan sopasCmdVec[CMD_SET_3_4_TO_ENCODER] = "\x02sWN DO3And4Fnc 1\x03"; //TODO remove this and add param sopasCmdVec[CMD_SET_ENOCDER_RES_1] = "\x02sWN LICencres 1\x03"; + + sopasCmdVec[CMD_SET_SCANDATACONFIGNAV] = "\x02sMN mLMPsetscancfg +2000 +1 +7500 +3600000 0 +2500 0 0 +2500 0 0 +2500 0 0\x03"; /* * Special configuration for NAV Scanner * in hex @@ -935,17 +937,14 @@ namespace sick_scan * */ sopasCmdVec[CMD_GET_ANGLE_COMPENSATION_PARAM] = "\x02sRN MCAngleCompSin\x03"; - - - sopasCmdVec[CMD_SET_SCANDATACONFIGNAV] = "\x02sMN mLMPsetscancfg +2000 +1 +7500 +3600000 0 +2500 0 0 +2500 0 0 +2500 0 0\x03"; - // defining cmd mask for cmds with variable input sopasCmdMaskVec[CMD_SET_PARTIAL_SCAN_CFG] = "\x02sMN mLMPsetscancfg %+d 1 %+d 0 0\x03";//scanfreq [1/100 Hz],angres [1/10000°], sopasCmdMaskVec[CMD_SET_PARTICLE_FILTER] = "\x02sWN LFPparticle %d %d\x03"; - sopasCmdMaskVec[CMD_SET_MEAN_FILTER] = "\x02sWN LFPmeanfilter %d +%d 1\x03"; + sopasCmdMaskVec[CMD_SET_MEAN_FILTER] = "\x02sWN LFPmeanfilter %d %d 0\x03"; sopasCmdMaskVec[CMD_ALIGNMENT_MODE] = "\x02sWN MMAlignmentMode %d\x03"; sopasCmdMaskVec[CMD_APPLICATION_MODE] = "\x02sWN SetActiveApplications 1 %s %d\x03"; sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES] = "\x02sWN LMPoutputRange 1 %X %X %X\x03"; + sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES_NAV3] = "\x02sWN LMPoutputRange 1 %X %X %X %X %X %X %X %X %X %X %X %X\x03"; //sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG]= "\x02sWN LMDscandatacfg %02d 00 %d 00 %d 0 %d 0 0 0 1 +1\x03"; //outputChannelFlagId,rssiFlag, rssiResolutionIs16Bit ,EncoderSetings sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 0 0 %02d 0 0 0 1 1\x03";//outputChannelFlagId,rssiFlag, rssiResolutionIs16Bit ,EncoderSetings /* @@ -1627,10 +1626,12 @@ namespace sick_scan pn.setParam("locationName", std::string(szLocationName)); } break; + + case CMD_GET_PARTIAL_SCANDATA_CFG: { - const char *strPtr = sopasReplyStrVec[CMD_LOCATION_NAME].c_str(); + const char *strPtr = sopasReplyStrVec[CMD_GET_PARTIAL_SCANDATA_CFG].c_str(); ROS_INFO("Config: %s\n", strPtr); } break; @@ -1817,9 +1818,24 @@ namespace sick_scan char requestOutputAngularRange[MAX_STR_LEN]; std::vector outputAngularRangeReply; - const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES].c_str(); - sprintf(requestOutputAngularRange, pcCmdMask, angleRes10000th, angleStart10000th, angleEnd10000th); + + bool NAV3xxOutputRangeSpecialHandling=false; + if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) + { + NAV3xxOutputRangeSpecialHandling=true; + const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES_NAV3].c_str(); + sprintf(requestOutputAngularRange, pcCmdMask, + angleRes10000th, angleStart10000th, angleEnd10000th, + angleRes10000th, angleStart10000th, angleEnd10000th, + angleRes10000th, angleStart10000th, angleEnd10000th, + angleRes10000th, angleStart10000th, angleEnd10000th); + } + else + { + const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES].c_str(); + sprintf(requestOutputAngularRange, pcCmdMask, angleRes10000th, angleStart10000th, angleEnd10000th); + } if (useBinaryCmd) { unsigned char tmpBuffer[255] = {0}; @@ -1833,10 +1849,28 @@ namespace sick_scan strcpy((char *) tmpBuffer, "WN LMPoutputRange "); unsigned short orgLen = strlen((char *) tmpBuffer); - colab::addIntegerToBuffer(tmpBuffer, orgLen, iStatus); - colab::addIntegerToBuffer(tmpBuffer, orgLen, angleRes10000th); - colab::addIntegerToBuffer(tmpBuffer, orgLen, angleStart10000th); - colab::addIntegerToBuffer(tmpBuffer, orgLen, angleEnd10000th); + if (NAV3xxOutputRangeSpecialHandling){ + colab::addIntegerToBuffer(tmpBuffer, orgLen, iStatus); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleRes10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleStart10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleEnd10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleRes10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleStart10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleEnd10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleRes10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleStart10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleEnd10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleRes10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleStart10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleEnd10000th); + } + else + { + colab::addIntegerToBuffer(tmpBuffer, orgLen, iStatus); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleRes10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleStart10000th); + colab::addIntegerToBuffer(tmpBuffer, orgLen, angleEnd10000th); + } sendLen = orgLen; colab::addFrameToBuffer(sendBuffer, tmpBuffer, &sendLen); @@ -2112,6 +2146,43 @@ namespace sick_scan ROS_ERROR("ang_res and scan_freq have to be set, only one param is set skiping scan_fre/ang_res config"); } } + // Config Mean filter + /* + char requestMeanSetting[MAX_STR_LEN]; + int meanFilterSetting = 0; + int MeanFilterActive=0; + pn.getParam("mean_filter", meanFilterSetting); // filter_echos + if(meanFilterSetting>2) + { + MeanFilterActive=1; + } + else{ + //needs to be at leas 2 even if filter is disabled + meanFilterSetting = 2; + } + // Uses sprintf-Mask to set bitencoded echos and rssi enable flag + const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_MEAN_FILTER].c_str(); + + //First echo : 0 + //All echos : 1 + //Last echo : 2 + + sprintf(requestMeanSetting, pcCmdMask, MeanFilterActive, meanFilterSetting); + std::vector outputFilterMeanReply; + + + if (useBinaryCmd) + { + std::vector reqBinary; + this->convertAscii2BinaryCmd(requestMeanSetting, &reqBinary); + result = sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_SET_ECHO_FILTER]); + } + else + { + result = sendSopasAndCheckAnswer(requestMeanSetting, &outputFilterMeanReply); + } + +*/ // CONFIG ECHO-Filter (only for MRS1000 not available for TiM5xx if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() >= 4) { @@ -3822,6 +3893,7 @@ namespace sick_scan std::string keyWord8 = "sWN TSCTCupdatetime"; std::string keyWord9 = "sWN TSCTCSrvAddr"; std::string keyWord10 = "sWN LICencres"; + std::string keyWord11 = "sWN LFPmeanfilter"; //BBB @@ -4033,7 +4105,21 @@ namespace sick_scan swap_endian(buffer, bufferLen); } - + if (cmdAscii.find(keyWord11) != std::string::npos) + { + char tmpStr[1024] = {0}; + char szApplStr[255] = {0}; + int keyWord11Len = keyWord11.length(); + int dummy0, dummy1,dummy2; + strcpy(tmpStr, requestAscii + keyWord11Len + 2); + sscanf(tmpStr, "%d %d %d", &dummy0, &dummy1, &dummy2); + // rebuild string + buffer[0] = dummy0 ? 0x01 : 0x00; + buffer[1] =dummy1/256;// + buffer[2] =dummy1%256;// + buffer[3] =dummy2; + bufferLen = 4; + } // copy base command string to buffer bool switchDoBinaryData = false; for (int i = 1; i <= (int) (msgLen); i++) // STX DATA ETX --> 0 1 2 diff --git a/include/sick_scan/sick_scan_common.h b/include/sick_scan/sick_scan_common.h index 0809b57..921b983 100644 --- a/include/sick_scan/sick_scan_common.h +++ b/include/sick_scan/sick_scan_common.h @@ -104,6 +104,7 @@ namespace sick_scan CMD_SET_ACCESS_MODE_3, CMD_SET_ACCESS_MODE_3_SAFETY_SCANNER, CMD_SET_OUTPUT_RANGES, + CMD_SET_OUTPUT_RANGES_NAV3, CMD_GET_OUTPUT_RANGES, CMD_RUN, CMD_SET_PARTIAL_SCAN_CFG, diff --git a/launch/sick_nav_2xx.launch b/launch/sick_nav_2xx.launch index 4125abd..ec22c28 100644 --- a/launch/sick_nav_2xx.launch +++ b/launch/sick_nav_2xx.launch @@ -7,7 +7,7 @@ - +