angle compensation of NAV3xx optimized

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})
 

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()

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

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 <algorithm> // for std::min
 

driver/src/sick_generic_parser.cpp

@@ -267,19 +267,19 @@ 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;
   }
 
   /*!
   \brief flag to mark mirroring of rotation direction
   \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\\");