Release 1.9.0 field monitoring for TiM7xx and TiM7xxS

SICKAG · Feb 15, 2021 · d1e6d93 · d1e6d93
1 parent d3634ac
commit d1e6d93
Show file tree

Hide file tree

Showing 12 changed files with 129 additions and 103 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -6,8 +6,8 @@
 cmake_minimum_required(VERSION 2.8.3)
 
 # build options: set OFF for relese version, ON for development and test
-option(ENABLE_EMULATOR "Build emulator for offline and unittests" OFF) # OFF (release) or ON (development)
-option(BUILD_DEBUG_TARGET "Build debug target" OFF) # OFF (release) or ON (development)
+option(ENABLE_EMULATOR "Build emulator for offline and unittests" ON) # OFF (release) or ON (development)
+option(BUILD_DEBUG_TARGET "Build debug target" ON) # OFF (release) or ON (development)
 
 # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
 # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wpedantic")

diff --git a/README.md b/README.md
@@ -213,7 +213,7 @@ The use of the parameters can be looked up in the launch files. This is also rec
 
 - Angle compensation: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](./doc/angular_compensation.md).
 
-- The **TiM7xxS** family has [extended settings for field monitoring](./doc/tim7xxs_extensions.md).
+- The **TiM7xx** and **TiM7xxS** families have [extended settings for field monitoring](./doc/tim7xxs_extensions.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/doc/tim7xxs_extensions.md b/doc/tim7xxs_extensions.md
@@ -1,12 +1,12 @@
 # Extensions for TiM7xxS
 
-The TiM7xxS family has the following extended settings for field monitoring:
+The TiM7xx and TiM7xxS families have the following extended settings for field monitoring:
 
 ## Field monitoring messages
 
-TiM7xxS scanner support field monitoring. Fields can be configured by Sopas ET. Once they are configured, sick_scan publishes ros messages containing the monitoring information from the lidar. 
+TiM7xx and TiM7xxS scanner support field monitoring. Fields can be configured by Sopas ET. Once they are configured, sick_scan publishes ros messages containing the monitoring information from the lidar. 
 
-By default, field monitoring is enabled in the launch file [sick_tim_7xxS.launch](../launch/sick_tim_7xxS.launch) by following settings:
+By default, field monitoring is enabled in the launch file [sick_tim_7xx.launch](../launch/sick_tim_7xx.launch) resp. [sick_tim_7xxS.launch](../launch/sick_tim_7xxS.launch) by following settings:
 ```
 <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
 <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->

diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp
@@ -332,21 +332,24 @@ namespace sick_scan
     return (useSafetyPasWD);
   }
 
-  bool ScannerBasicParam::getUseSafetyFields(){
-    return(useSafetyFields);
+  EVAL_FIELD_SUPPORT ScannerBasicParam::getUseEvalFields()
+  {
+    return this->useEvalFields;
   }
 
-  void ScannerBasicParam::setUseSafetyFields(bool _useSafetyFields){
-    this->useSafetyFields=_useSafetyFields;
+  void ScannerBasicParam::setUseEvalFields(EVAL_FIELD_SUPPORT _useEvalFields)
+  {
+    this->useEvalFields = _useEvalFields;
   }
+
   /*!
   \brief Construction of parameter object
 
   */
   ScannerBasicParam::ScannerBasicParam()
   : numberOfLayers(0), numberOfShots(0), numberOfMaximumEchos(0), elevationDegreeResolution(0), angleDegressResolution(0), expectedFrequency(0),
      useBinaryProtocol(false), IntensityResolutionIs16Bit(false), deviceIsRadar(false), useSafetyPasWD(false), encoderMode(0),
-     CartographerCompatibility(false), scanMirroredAndShifted(false), useSafetyFields(false)
+     CartographerCompatibility(false), scanMirroredAndShifted(false), useEvalFields(EVAL_FIELD_UNSUPPORTED)
   {
     this->elevationDegreeResolution = 0.0;
     this->setUseBinaryProtocol(false);
@@ -420,7 +423,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) ==
           0)  // LMS1000 - 4 layer, 1101 shots per scan
@@ -436,7 +439,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_240_NAME) ==
           0) // TIM_5xx - 1 Layer, max. 811 shots per scan
@@ -451,7 +454,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
 
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_5XX_NAME) ==
@@ -467,8 +470,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
-
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_4XXX_NAME) == 0) // LMS_4xxx - 1 Layer, 600 Hz
       {
@@ -482,7 +484,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XX_NAME) == 0) // TIM_7xx - 1 Layer Scanner
       {
@@ -496,7 +498,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(USE_EVAL_FIELD_TIM7XX_LOGIC);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) // TIM_7xxS - 1 layer Safety Scanner
       {
@@ -510,7 +512,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(true); // Safety scanner
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(true);
+        basicParams[i].setUseEvalFields(USE_EVAL_FIELD_TIM7XX_LOGIC);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) // LMS_5xx - 1 Layer
       {
@@ -524,7 +526,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XX_NAME) == 0) // LMS_1xx - 1 Layer
       {
@@ -538,7 +540,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_MRS_6XXX_NAME) == 0) //
       {
@@ -553,7 +555,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
 
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_3XX_NAME) == 0) // Radar
@@ -569,7 +571,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // Nav 3xx
       {
@@ -581,7 +583,7 @@ namespace sick_scan
         basicParams[i].setUseBinaryProtocol(true);
         basicParams[i].setDeviceIsRadar(false); // Default
         basicParams[i].setScanMirroredAndShifted(true);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0) // NAV_2xx - 1 Layer
       {
@@ -595,7 +597,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_4XX_NAME) == 0) // TiM433 and TiM443
       {
@@ -609,7 +611,7 @@ namespace sick_scan
         basicParams[i].setUseSafetyPasWD(false); // Default
         basicParams[i].setEncoderMode(-1); // Default
         basicParams[i].setScanMirroredAndShifted(false);
-        basicParams[i].setUseSafetyFields(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
       }
     }
 

diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp
@@ -371,7 +371,7 @@ namespace sick_scan
     publish_lferec_ = false;
     publish_lidoutputstate_ = false;
     const std::string scannername = parser_->getCurrentParamPtr()->getScannerName();
-    if (scannername.compare(SICK_SCANNER_TIM_7XXS_NAME) == 0)
+    if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC)
     {
       lferec_pub_ = nh_.advertise<sick_scan::LFErecMsg>(scannername + "/lferec", 100);
       lidoutputstate_pub_ = nh_.advertise<sick_scan::LIDoutputstateMsg>(scannername + "/lidoutputstate", 100);
@@ -435,7 +435,7 @@ namespace sick_scan
       printf("\nSOPAS - Stopped streaming scan data.\n");
     }
 
-    if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0)
+    if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC)
     {
       if(sendSOPASCommand("\x02sEN LFErec 0\x03", NULL) != 0 // TiM781S: deactivate LFErec messages, send "sEN LFErec 0"
       || sendSOPASCommand("\x02sEN LIDoutputstate 0\x03", NULL) != 0 // TiM781S: deactivate LIDoutputstate messages, send "sEN LIDoutputstate 0"
@@ -1170,13 +1170,11 @@ namespace sick_scan
 
     bool rssiFlag = false;
     bool rssiResolutionIs16Bit = true; //True=16 bit Flase=8bit
-    // bool useSafetyfields=false;
     int activeEchos = 0;
     ros::NodeHandle pn("~");
 
     pn.getParam("intensity", rssiFlag);
     pn.getParam("intensity_resolution_16bit", rssiResolutionIs16Bit);
-    // pn.getParam("use_safety_fields", useSafetyfields);
     //check new ip adress and add cmds to write ip to comand chain
     std::string sNewIPAddr = "";
     boost::asio::ip::address_v4 ipNewIPAddr;
@@ -1215,7 +1213,6 @@ namespace sick_scan
     }
 
     this->parser_->getCurrentParamPtr()->setIntensityResolutionIs16Bit(rssiResolutionIs16Bit);
-    // this->parser_->getCurrentParamPtr()->setUseSafetyFields(useSafetyfields);
     // parse active_echos entry and set flag array
     pn.getParam("active_echos", activeEchos);
 
@@ -2106,7 +2103,7 @@ namespace sick_scan
 
       }
       //SAFTY FIELD PARSING
-      if (this->parser_->getCurrentParamPtr()->getUseSafetyFields())
+      if (this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC)
       {
         ROS_INFO("Reading safety fields");
         SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance();
@@ -2443,7 +2440,7 @@ namespace sick_scan
       startProtocolSequence.push_back(CMD_RUN);  // leave user level
       startProtocolSequence.push_back(CMD_START_SCANDATA);
 
-      if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0)
+      if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC)
       {
 
         // Activate LFErec, LIDoutputstate and LIDinputstate messages

diff --git a/include/sick_scan/sick_generic_parser.h b/include/sick_scan/sick_generic_parser.h
@@ -58,6 +58,15 @@
 // namespace sensor_msgs
 namespace sick_scan
 {
+  enum EVAL_FIELD_SUPPORT // type of eval field support:
+  {
+    EVAL_FIELD_UNSUPPORTED = 0,        // Lidar does not support eval field (default)
+    USE_EVAL_FIELD_TIM7XX_LOGIC,       // eval fields supported by TiM7xx and TiM7xxS
+    USE_EVAL_FIELD_LMS5XX_LOGIC,       // eval fields supported by LMS5XX
+    USE_EVAL_FIELD_LMS5XX_UNSUPPORTED, // eval fields not supported by LMS5XX
+    USE_EVAL_FIELD_NUM                 // max number of eval field support types
+  };
+
   class ScannerBasicParam
   {
   public:
@@ -115,9 +124,9 @@ namespace sick_scan
 
     int8_t getEncoderMode();
 
-    bool getUseSafetyFields();
+    EVAL_FIELD_SUPPORT getUseEvalFields();
 
-    void setUseSafetyFields(bool _useSafetyFields);
+    void setUseEvalFields(EVAL_FIELD_SUPPORT _useEvalFields);
 
   private:
     std::string scannerName;
@@ -134,7 +143,7 @@ namespace sick_scan
     int8_t encoderMode;
     bool CartographerCompatibility;
     bool scanMirroredAndShifted;
-    bool useSafetyFields;
+    EVAL_FIELD_SUPPORT useEvalFields;
   };
 
 

diff --git a/launch/sick_tim_7xx.launch b/launch/sick_tim_7xx.launch
@@ -36,6 +36,10 @@
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
+        <param name="start_services" type="bool" value="True"/> <!-- start ros service for cola commands -->
+        <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
+        <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
+        <param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->
     </node>
 
 </launch>

diff --git a/launch/sick_tim_7xxS.launch b/launch/sick_tim_7xxS.launch
@@ -36,7 +36,7 @@
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
-        <param name="start_services" type="bool" value="True"/>
+        <param name="start_services" type="bool" value="True"/> <!-- start ros service for cola commands -->
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
         <param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->

diff --git a/test/emulator/config/rviz_emulator_cfg.rviz b/test/emulator/config/rviz_emulator_cfg.rviz
@@ -8,6 +8,7 @@ Panels:
         - /PointCloud21
         - /Axes1
         - /MarkerArray1
+        - /MarkerArray2
       Splitter Ratio: 0.5
     Tree Height: 799
   - Class: rviz/Selection
@@ -70,7 +71,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 39172
+      Max Intensity: 38513
       Min Color: 0; 0; 0
       Min Intensity: 0
       Name: PointCloud2
@@ -94,12 +95,20 @@ Visualization Manager:
       Value: true
     - Class: rviz/MarkerArray
       Enabled: true
-      Marker Topic: /sick_tim_7xxS/marker
+      Marker Topic: /sick_tim_7xx/marker
       Name: MarkerArray
       Namespaces:
         sick_scan: true
       Queue Size: 100
       Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /sick_tim_7xxS/marker
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48