Merge pull request #68 from SICKAG/feature/lms511-min_max_angles

SICKAG · May 26, 2022 · d6b819a · d6b819a
1 parent 9af342c
commit d6b819a
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -8,6 +8,9 @@ features that will be removed in future versions **Removed** for deprecated feat
 
 ## Released ##
 
+### v2.6.7 - 
+  - **Update** LMS511 configuration #67
+
 ### v2.6.6 - 
   - **Update** NAV310 + LRS4xxx update, issues #58, #59, #60, #61
 

diff --git a/driver/src/sick_generic_caller.cpp b/driver/src/sick_generic_caller.cpp
@@ -81,7 +81,7 @@
 
 #define SICK_GENERIC_MAJOR_VER "2"
 #define SICK_GENERIC_MINOR_VER "6"
-#define SICK_GENERIC_PATCH_LEVEL "6"
+#define SICK_GENERIC_PATCH_LEVEL "7"
 
 #include <algorithm> // for std::min
 

diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp
@@ -486,6 +486,12 @@ namespace sick_scan
       config_.min_ang = -M_PI;
       config_.max_ang = +M_PI;
     }
+    else if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0)
+    {
+      // LMS min/max angles currently set to +-95 degree
+      config_.min_ang = -95.0 * M_PI / 180.0;
+      config_.max_ang = +95.0 * M_PI / 180.0;
+    }
 
     // datagram publisher (only for debug)
     rosDeclareParam(nh, "publish_datagram", false);
@@ -554,6 +560,16 @@ namespace sick_scan
         config_.max_ang = +M_PI;
       }
     }
+    else if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0)
+    {
+      // LMS min/max angles currently set to +-95 degree
+      if(std::abs(config_.min_ang + 95.0 * M_PI / 180.0) > FLT_EPSILON || std::abs(config_.max_ang - 95.0 * M_PI / 180.0) > FLT_EPSILON)
+      {
+        ROS_WARN_STREAM("## WARNING: configured min/max_angle = " << config_.min_ang << "," << config_.max_ang << " not supported. min/max_angle = -95,+95 degree will be used.");
+        config_.min_ang = -95.0 * M_PI / 180.0;
+        config_.max_ang = +95.0 * M_PI / 180.0;
+      }
+    }
 
     cloud_marker_ = 0;
     publish_lferec_ = false;