Dev 9928 make first mvp of improved laser scanner driver

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_launch.py

@@ -30,7 +30,7 @@ def expand_param_file_name(context):
     launch_description.add_action(param_file_path)
 
     hokuyo_node = Node(
-        package='urg_node', node_executable='urg_node', output='screen',
+        package='urg_node', executable='urg_node_driver', output='screen',
         parameters=[LaunchConfiguration('param')]
         )
 

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

@@ -315,7 +315,6 @@ bool URGCWrapper::grabScan(sensor_msgs::msg::MultiEchoLaserScan & msg)
 
   return true;
 }
-
 bool URGCWrapper::getAR00Status(URGStatus & status)
 {
   // Construct and write AR00 command.
@@ -338,11 +337,11 @@ bool URGCWrapper::getAR00Status(URGStatus & status)
   response.erase(0, 1);
   response.erase(response.size() - 1, 1);
 
+  std::string s;
+
   // Get the CRC, it's the last 4 chars.
-  std::stringstream ss;
-  ss << response.substr(response.size() - 4, 4);
-  uint16_t crc;
-  ss >> std::hex >> crc;
+  s = response.substr(response.size() - 4, 4);
+  const uint16_t crc = std::stoul(s, nullptr, 16);
 
   // Remove the CRC from the check.
   std::string msg = response.substr(0, response.size() - 4);
@@ -355,55 +354,80 @@ bool URGCWrapper::getAR00Status(URGStatus & status)
   }
 
   // Debug output reponse up to scan data.
-  RCLCPP_DEBUG(logger_, "Response: %s", response.substr(0, 41).c_str());
-  // Decode the result if crc checks out.
-  // Grab the status
-  ss.clear();
-  RCLCPP_DEBUG(logger_, "Status: %s", response.substr(8, 2).c_str());
-  ss << response.substr(8, 2);  // Status is 8th position 2 chars.
-  ss >> std::hex >> status.status;
+  RCLCPP_DEBUG(logger_, "Response: %s", response.substr(0, 44).c_str());
 
+  // Grab the status
+  s = response.substr(8, 2);
+  status.status = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Status: 0x%s = %d", s.c_str(), status.status);
   if (status.status != 0) {
     RCLCPP_WARN(logger_, "Received bad status");
     return false;
   }
-
   // Grab the operating mode
-  ss.clear();
-  RCLCPP_DEBUG(logger_, "Operating mode: %s", response.substr(10, 1).c_str());
-  ss << response.substr(10, 1);
-  ss >> std::hex >> status.operating_mode;
+  s = response.substr(10, 1);
+  status.operating_mode = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Operating mode: 0x%s = %d", s.c_str(), status.operating_mode);
 
   // Grab the area number
-  ss.clear();
-  ss << response.substr(11, 2);
-  RCLCPP_DEBUG(logger_, "Area Number: %s", response.substr(11, 2).c_str());
-  ss >> std::hex >> status.area_number;
-  // Per documentation add 1 to offset area number
-  status.area_number++;
+  s = response.substr(11, 2);
+  status.area_number = std::stoul(s, nullptr, 16);
+  status.area_number++;   // adding 1 according to documentation
+  RCLCPP_DEBUG(logger_, "Area number: 0x%s = %d (+1)", s.c_str(), status.area_number);
 
   // Grab the Error Status
-  ss.clear();
-  ss << response.substr(13, 1);
-  RCLCPP_DEBUG(logger_, "Error status: %s", response.substr(13, 1).c_str());
-  ss >> std::hex >> status.error_status;
-
+  s = response.substr(13, 1);
+  status.error_status = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Error status: 0x%s = %d", s.c_str(), status.error_status);
 
   // Grab the error code
-  ss.clear();
-  ss << response.substr(14, 2);
-  RCLCPP_DEBUG(logger_, "Error code: %s", response.substr(14, 2).c_str());
-  ss >> std::hex >> status.error_code;
+  s = response.substr(14, 2);
+  status.error_code = std::stoul(s, nullptr, 16);
   // Offset by 0x40 is non-zero as per documentation
   if (status.error_code != 0) {
     status.error_code += 0x40;
   }
+  RCLCPP_DEBUG(logger_, "Error code: 0x%s = 0x%x (+0x40 if > 0) = %d",
+          s.c_str(), status.error_code, status.error_code);
 
   // Get the lockout status
-  ss.clear();
-  ss << response.substr(16, 1);
-  RCLCPP_DEBUG(logger_, "Lockout: %s", response.substr(16, 1).c_str());
-  ss >> std::hex >> status.lockout_status;
+  status.lockout_status = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Lockout: 0x%s = %d", s.c_str(), status.lockout_status);
+
+  uint16_t unused0;
+
+  s = response.substr(17, 1);
+  unused0 = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "OSSD 1: 0x%s = %d", s.c_str(), unused0);
+
+  s = response.substr(18, 1);
+  unused0 = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "OSSD 2: 0x%s = %d", s.c_str(), unused0);
+
+  s = response.substr(19, 1);
+  unused0 = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Warning 1: 0x%s = %d", s.c_str(), unused0);
+
+  s = response.substr(20, 1);
+  unused0 = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Warning 2: 0x%s = %d", s.c_str(), unused0);
+
+  s = response.substr(33, 8);
+  RCLCPP_DEBUG(logger_, "Timestamp: 0x%s", s.c_str());
+
+  s = response.substr(41, 1);
+  unused0 = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Laser off: 0x%s = %d", s.c_str(), unused0);
+
+  // Get optical window contamination warning
+  s = response.substr(42, 1);
+  status.optical_window_contaminated = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Optical window contaminated: 0x%s = %d",
+          s.c_str(), status.optical_window_contaminated);
+
+  s = response.substr(43, 1);
+  unused0 = std::stoul(s, nullptr, 16);
+  RCLCPP_DEBUG(logger_, "Encoder pattern: 0x%s = %d", s.c_str(), unused0);
 
   return true;
 }

src/urg_node.cpp

@@ -36,6 +36,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <mutex>
 
 namespace urg_node
 {
@@ -165,6 +166,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;
@@ -612,7 +614,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;