improved angle compensation

driver/src/helper/angle_compensator.cpp

@@ -64,7 +64,7 @@
 #include <sstream>
 #include <iostream>
 #include <math.h>
-
+#include <assert.h>
 
 using namespace std;
 
@@ -104,7 +104,12 @@ using namespace std;
 double AngleCompensator::compensateAngleInRad(double angleInRad)
 {
   double deg2radFactor = 0.01745329252; // pi/180 deg - see for example: https://www.rapidtables.com/convert/number/degrees-to-radians.html
-  double angleCompInRad = angleInRad + deg2radFactor * amplCorr * sin(angleInRad + phaseCorrInRad) + offsetCorrInRad;
+  int sign = 1;
+  if (useNegSign)
+  {
+    sign = -1;
+  }
+  double angleCompInRad = angleInRad + deg2radFactor * amplCorr * sin(angleInRad + sign * phaseCorrInRad) + offsetCorrInRad;
   return(angleCompInRad);
 }
 
@@ -114,12 +119,17 @@ double AngleCompensator::compensateAngleInRad(double angleInRad)
 */
 double AngleCompensator::compensateAngleInDeg(double angleInDeg)
 {
+  int sign = 1;
+  if (useNegSign)
+  {
+    sign = -1;
+  }
   // 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
   double angleRawInRad = deg2radFactor * angleInDeg;
   double phaseCorrInRad= deg2radFactor * phaseCorrInDeg;
-  angleCompInDeg = angleInDeg + amplCorr * sin(angleRawInRad + phaseCorrInRad) + offsetCorrInDeg;
+  angleCompInDeg = angleInDeg + amplCorr * sin(angleRawInRad + sign * phaseCorrInRad) + offsetCorrInDeg;
   return(angleCompInDeg);
 }
 
@@ -192,15 +202,34 @@ int AngleCompensator::parseAsciiReply(const char *replyStr)
 */
 int AngleCompensator::parseReply(bool isBinary, std::vector<unsigned char>& replyVec)
 {
+
   int retCode = 0;
-std::string stmp;
+  std::string stmp;
+  int payLoadLen = 0;
   if (isBinary) // convert binary message into the ASCII format to reuse parsing algorithm
   {
     stmp = "";
     int sLen = replyVec.size();
-    int offset = sLen - 12;
+    assert((sLen == 40) || (sLen == 36));
+
+    switch(sLen)
+    {
+      case 36:
+        useNegSign = true; // phase compensation is negative for 310 - using the short telegram
+        payLoadLen = 8;
+        break;
+      case 40:
+        payLoadLen = 12;
+        break;
+      default:
+        assert(0);
+        break;
+    }
+
+    int offset = sLen - payLoadLen - 1; // -1 for CRC
+    assert(replyVec[offset-1] == 0x20); // must be a space there - last test
     int relCnt = 0;
-    for (int i = 0; i < sLen; i++)
+    for (int i = 8; i < sLen - 1; i++)
     {
       if (i < offset)
       {
@@ -212,21 +241,51 @@ std::string stmp;
         sprintf(szTmp,"%02X", replyVec[i]);
         relCnt++;
         stmp += szTmp;
-        if (relCnt < 12)
+
+        int posCutArr[2] = {4,8};
+        if (payLoadLen == 8)
+        {
+          posCutArr[0] = 2;
+          posCutArr[1] = 6;
+
+        }
+        if (relCnt < payLoadLen)
         {
-          if ((relCnt % 4) == 0)
+          for (int k = 0; k < 2; k++)
           {
-             stmp += ' ';
+            if (posCutArr[k] == relCnt)
+            {
+              stmp += ' ';
+            }
           }
         }
-
       }
     }
   }
+  else
+  {
+     for (int i = 0; i <  replyVec.size(); i++)
+     {
+       stmp += (char)replyVec[i];
+     }
+  }
   parseAsciiReply(stmp.c_str());
   return(retCode);
 }
 
+
+std::string AngleCompensator::getHumanReadableFormula(void)
+{
+  char szDummy[1024] = {0};
+  std::string s;
+
+  sprintf(szDummy,"Angle[comp.] = Angle[Raw] + %8.6lf * sin(Angle[Raw] %c %8.6lf [deg]) +  %8.6lf",
+          amplCorr, useNegSign ? '-' : '+', phaseCorrInDeg, offsetCorrInDeg);
+
+  s  = szDummy;
+  return(s);
+
+}
 /*!
 \brief Testbed for angle compensation
 */
@@ -236,16 +295,61 @@ void AngleCompensator::testbed()
   std::vector<unsigned char> testVec;
 
   std::string s = string("sRA MCAngleCompSin ");
-  for (int i = 0; i < s.length(); i++)
-  {
-    testVec.push_back(s[i]);
-  }
-  unsigned char dataArr[] = {0x00,0x00,0x07,0x65,0xff,0xfc,0xc9,0xb9,0xff,0xff,0xff,0x0b };
-  for (int i = 0; i < sizeof(dataArr)/sizeof(dataArr[0]); i++)
+
+  for (int iLoop = 0; iLoop < 2; iLoop++)
   {
-    testVec.push_back(dataArr[i]);
+    testVec.clear(); // start with empty test vector
+    switch(iLoop)
+    {
+      case 0: // test for NAV2XX
+      {
+        unsigned char preFix[8] = {0x02,0x02,0x02,0x02,0x00,0x00,0x00,27+4};
+        for (int i = 0; i < 8; i++)
+        {
+          testVec.push_back(preFix[i]);
+        }
+        // TODO CHECK IT!! 0xFF appended
+        unsigned char dataArr[] = {0x00,0x00,0x07,0x65,0xff,0xfc,0xc9,0xb9,0xff,0xff,0xff,0x0b,0xFF};
+        for (int i = 0; i < s.length(); i++)
+        {
+          testVec.push_back(s[i]);
+        }
+        for (int i = 0; i < sizeof(dataArr)/sizeof(dataArr[0]); i++)
+        {
+          testVec.push_back(dataArr[i]);
+        }
+        break;
+      }
+      case 1:
+      {
+        unsigned char preFix[8] = {0x02,0x02,0x02,0x02,0x00,0x00,0x00,27};
+        for (int i = 0; i < 8; i++)
+        {
+          testVec.push_back(preFix[i]);
+        }
+        for (int i = 0; i < s.length(); i++)
+        {
+          testVec.push_back(s[i]);
+        }
+        unsigned char dataArr[] = {0x03,	0x37,	0x00,	0x1d,	0x8e,	0x8d,	0x00,	0xe7,	0x87};
+        for (int i = 0; i < sizeof(dataArr)/sizeof(dataArr[0]); i++)
+        {
+          testVec.push_back(dataArr[i]);
+        }
+
+        break;
+      }
+
+    }
+    ac.parseReply(true,testVec);
+    printf("Formula used: %s\n", ac.getHumanReadableFormula().c_str());
+
   }
-  ac.parseReply(true,testVec);
+
+
+
+
+
 
 
   testVec.clear();

driver/src/sick_generic_caller.cpp

@@ -118,10 +118,11 @@
 // 1.7.0: 2020-06-01: TiM443 added
 // 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
 
 #define SICK_GENERIC_MAJOR_VER "1"
 #define SICK_GENERIC_MINOR_VER "7"
-#define SICK_GENERIC_PATCH_LEVEL "2"
+#define SICK_GENERIC_PATCH_LEVEL "3"
 
 #include <algorithm> // for std::min
 
@@ -140,7 +141,7 @@ std::string getVersionInfo();
 */
 int main(int argc, char **argv)
 {
-  // AngleCompensator ac;
+  // AngleCompensator ac(false);
   // ac.testbed();
 
   DataDumper::instance().writeToFileNameWhenBufferIsFull("/tmp/sickscan_debug.csv");

driver/src/sick_scan_common.cpp

@@ -1043,7 +1043,16 @@ namespace sick_scan
     /*
      * NAV2xx supports angle compensation
      */
+    bool isNav2xxOr3xx = false;
     if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0)
+    {
+      isNav2xxOr3xx = true;
+    }
+    if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0)
+    {
+      isNav2xxOr3xx = true;
+    }
+    if (isNav2xxOr3xx)
     {
       sopasCmdChain.push_back(CMD_GET_ANGLE_COMPENSATION_PARAM);
     }
@@ -1628,10 +1637,30 @@ namespace sick_scan
 
         case CMD_GET_ANGLE_COMPENSATION_PARAM:
           {
-            this->angleCompensator = new AngleCompensator();
+            bool useNegSign = false;
+            if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0)
+            {
+              useNegSign = true; // use negative phase compensation for NAV3xx
+            }
+
+            this->angleCompensator = new AngleCompensator(useNegSign);
             std::string s = sopasReplyStrVec[CMD_GET_ANGLE_COMPENSATION_PARAM];
-            angleCompensator->parseReply(useBinaryCmd, sopasReplyBinVec[CMD_GET_ANGLE_COMPENSATION_PARAM]);
+            std::vector<unsigned char> tmpVec;
+
+            if (useBinaryCmd == false)
+            {
+              for (int i = 0; i < s.length(); i++)
+              {
+                tmpVec.push_back((unsigned char)s[i]);
+              }
+            }
+            else
+            {
+              tmpVec = sopasReplyBinVec[CMD_GET_ANGLE_COMPENSATION_PARAM];
+            }
+            angleCompensator->parseReply(useBinaryCmd, tmpVec);
 
+            ROS_INFO("Angle Comp. Formula used: %s\n", angleCompensator->getHumanReadableFormula().c_str());
           }
           break;
           // if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0)

include/sick_scan/helper/angle_compensator.h

@@ -7,22 +7,32 @@
 
 #include <string>
 #include <vector>
-
+#include <assert.h>
 class AngleCompensator
 {
 public:
   double compensateAngleInRad(double angleInRad);
   double compensateAngleInDeg(double angleInDeg);
   int parseAsciiReply(const char *asciiReply);
   int parseReply(bool isBinary, std::vector<unsigned char>& replyVec);
+  std::string getHumanReadableFormula(void);
   void testbed();
+  AngleCompensator()
+  {
+    assert(0); // forbidden!
+  }
+  AngleCompensator(bool _useNegSign)
+  {
+    useNegSign = _useNegSign;
+  }
 private:
 
   double amplCorr;
   double phaseCorrInDeg;
   double offsetCorrInDeg;
   double phaseCorrInRad;
   double offsetCorrInRad;
+  bool useNegSign; // for NAV310
 
 };
 

launch/sick_nav_3xx_ascii.launch

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+
+<launch>
+    <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
+    <node name="sick_nav_3xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen">
+        <param name="scanner_type" type="string" value="sick_nav_3xx"/>
+
+        <!-- -90° -->
+        <param name="min_ang" type="double" value="-1.570796327"/>
+
+        <!-- 270° -->
+        <param name="max_ang" type="double" value="4.71238898"/>
+
+        <param name="use_binary_protocol" type="bool" value="false"/>
+        <param name="intensity" type="bool" value="True"/>
+        <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
+        <param name="port" type="string" value="2112"/>
+        <param name="timelimit" type="int" value="5"/>
+    </node>
+
+</launch>

launch/sick_tim_4xx.launch

@@ -1,12 +1,14 @@
 <?xml version="1.0"?>
 <!--
-     *******************************************************************
-     Launch File for TiM 4xx lidar family (convering TiM 433 and TiM 443
-     *******************************************************************
+     **********************************************
+     Launch File for TiM 4xx lidar family
+     
+     Family covers TiM 433 and TiM 443.
+     **********************************************
      Start and stop angle is given in [rad]
 
-     Default min_angle is  -120 degree.
-     Default max_angle is  +120 degree.
+     Default min_angle is  -120 degree (-2.09439510 rad).
+     Default max_angle is  +120 degree (+2.09439510 rad).
 
      Axis orientation is given in ROS standard notation.
 
@@ -35,15 +37,15 @@
 
 <!-- You can launch the lidar on a specific ip address (e.g. 192.68.0.71) using the following example call:
 
-     roslaunch sick_scan sick_tim_4xx.launch hostname:=192.168.0.71
+     roslaunch sick_scan sick_tim_443.launch hostname:=192.168.0.71
 
 -->
 
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
     <arg name="cloud_topic" default="cloud"/>
     <arg name="frame_id" default="cloud"/>
-    <node name="sick_tim_4xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen">
+    <node name="sick_tim_443" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen">
         <!--<param name="robot_description" command="$(find xacro)/xacro.py '$(find sick_scan)/urdf/example.urdf.xacro'" />-->
         <param name="scanner_type" type="string" value="sick_tim_4xx"/>
         <!-- -120° -->