Dev 9928 make first mvp of improved laser scanner driver

DEBIAN/control

@@ -0,0 +1,8 @@
+Package: ros-galactic-urg-node
+Version: 102-patricdev
+Maintainer: Patrick Jakubowski <[email protected]>
+Description: urg_node with optical_window_contamination in topic (patrickdev)
+Section: misc
+Depends: libc6 (>= 2.14), libgcc-s1 (>= 3.0), libstdc++6 (>= 9), ros-galactic-console-bridge-vendor, libboost-dev, ros-galactic-builtin-interfaces, ros-galactic-diagnostic-updater, ros-galactic-laser-proc, ros-galactic-rclcpp, ros-galactic-rclcpp-components, ros-galactic-rosidl-default-generators, ros-galactic-sensor-msgs, ros-galactic-std-srvs, ros-galactic-urg-c, ros-galactic-urg-node-msgs
+Priority: optional
+Architecture: amd64

include/urg_node/urg_c_wrapper.hpp

@@ -63,6 +63,7 @@ class URGStatus
     error_status = false;
     error_code = 0;
     lockout_status = false;
+    optical_window_contaminated = false;
   }
 
   uint16_t status;
@@ -71,6 +72,7 @@ class URGStatus
   bool error_status;
   uint16_t error_code;
   bool lockout_status;
+  bool optical_window_contaminated;
 };
 
 class UrgDetectionReport

launch/urg_node_ethernet.yaml

@@ -11,7 +11,7 @@ urg_node:
     diagnostics_tolerance: 0.05
     diagnostics_window_time: 5.0
     error_limit: 4
-    get_detailed_status: false
+    get_detailed_status: true
     publish_intensity: false
     publish_multiecho: false
     cluster: 1

launch/urg_node_serial.yaml

@@ -11,7 +11,7 @@ urg_node:
     diagnostics_tolerance: 0.05
     diagnostics_window_time: 5.0
     error_limit: 4
-    get_detailed_status: false
+    get_detailed_status: true
     publish_intensity: false
     publish_multiecho: false
     cluster: 1

src/urg_c_wrapper.cpp

@@ -350,7 +350,7 @@ bool URGCWrapper::getAR00Status(URGStatus & status)
   }
 
   // Debug output reponse up to scan data.
-  RCLCPP_DEBUG(logger_, "Response: %s", response.substr(0, 41).c_str());
+  RCLCPP_DEBUG(logger_, "Response: %s", response.substr(0, 44).c_str());
   // Decode the result if crc checks out.
   // Grab the status
   ss.clear();
@@ -400,6 +400,42 @@ bool URGCWrapper::getAR00Status(URGStatus & status)
   RCLCPP_DEBUG(logger_, "Lockout: %s", response.substr(16, 1).c_str());
   ss >> std::hex >> status.lockout_status;
 
+  ss.clear();
+  ss << response.substr(17, 1);
+  RCLCPP_DEBUG(logger_, "OSSD 1 State: %s", response.substr(17, 1).c_str());
+
+  ss.clear();
+  ss << response.substr(18, 1);
+  RCLCPP_DEBUG(logger_, "OSSD 2 State: %s", response.substr(18, 1).c_str());
+
+  ss.clear();
+  ss << response.substr(19, 1);
+  RCLCPP_DEBUG(logger_, "Warning 1 State: %s", response.substr(19, 1).c_str());
+
+  ss.clear();
+  ss << response.substr(20, 1);
+  RCLCPP_DEBUG(logger_, "Warning 2 State: %s", response.substr(20, 1).c_str());
+
+  ss.clear();
+  ss << response.substr(33, 8);
+  RCLCPP_DEBUG(logger_, "Time Stamp: %s", response.substr(33, 8).c_str());
+
+  ss.clear();
+  ss << response.substr(41, 1);
+  RCLCPP_DEBUG(logger_, "Laser Off: %s", response.substr(41, 1).c_str());
+
+  // Get optical window contamination warning
+  {
+    std::stringstream ss2;
+    ss2 << response.substr(42, 1);
+    RCLCPP_DEBUG(logger_, "Opt. Window Contamination: \"%s\"", response.substr(42, 1).c_str());
+    ss2 >> std::hex >> status.optical_window_contaminated;
+  }
+
+  ss.clear();
+  ss << response.substr(43, 1);
+  RCLCPP_DEBUG(logger_, "Encoder Pattern: %s", response.substr(43, 1).c_str());
+
   return true;
 }
 

src/urg_node.cpp

@@ -36,6 +36,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <mutex>
 
 namespace urg_node
 {
@@ -162,6 +163,7 @@ bool UrgNode::updateStatus()
         msg.error_status = status.error_status;
         msg.error_code = status.error_code;
         msg.lockout_status = status.lockout_status;
+        msg.optical_window_contaminated = status.optical_window_contaminated;
 
         lockout_status_ = status.lockout_status;
         error_code_ = status.error_code;
@@ -340,7 +342,7 @@ void UrgNode::updateDiagnostics()
 {
   while (!close_diagnostics_) {
     diagnostic_updater_.force_update();
-    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
   }
 }
 
@@ -605,7 +607,8 @@ void UrgNode::run()
 
   //// Now that we are setup, kick off diagnostics.
   close_diagnostics_ = false;
-  diagnostics_thread_ = std::thread(std::bind(&UrgNode::updateDiagnostics, this));
+  //stop running a separate thread
+  //diagnostics_thread_ = std::thread(std::bind(&UrgNode::updateDiagnostics, this));
 
   // Start scanning now that everything is configured.
   close_scan_ = false;