Add new status information

include/urg_node/urg_c_wrapper.hpp

@@ -63,6 +63,12 @@ class URGStatus
     error_status = false;
     error_code = 0;
     lockout_status = false;
+    ossd_1 = false;
+    ossd_2 = false;
+    warn_1 = false;
+    warn_2 = false;
+    ossd_3 = false;
+    ossd_4 = false;
   }
 
   uint16_t status;
@@ -71,6 +77,12 @@ class URGStatus
   bool error_status;
   uint16_t error_code;
   bool lockout_status;
+  bool ossd_1;
+  bool ossd_2;
+  bool warn_1;
+  bool warn_2;
+  bool ossd_3;
+  bool ossd_4;
 };
 
 class UrgDetectionReport

launch/urg_lidar.launch.xml

@@ -0,0 +1,15 @@
+<launch>
+
+  <!-- A simple launch file for the urg_node package. -->
+
+  <!--  When using an IP-connected LIDAR, populate the "ip_address" parameter with the address of the LIDAR.
+      Otherwise, leave it blank. If supported by your LIDAR, you may enable the publish_intensity
+      and/or publish_multiecho options. -->
+
+  <arg name="param_file" default="$(find-pkg-share urg_node)/launch/urg_node_serial.yaml" />
+
+  <node name="urg_node" pkg="urg_node" exec="urg_node_driver" output="screen">
+    <param from="$(var param_file)" />
+  </node>
+
+</launch>

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')]
         )
 

src/urg_c_wrapper.cpp

@@ -398,6 +398,42 @@ bool URGCWrapper::getAR00Status(URGStatus & status)
   RCLCPP_DEBUG(logger_, "Lockout: %s", response.substr(16, 1).c_str());
   ss >> std::hex >> status.lockout_status;
 
+  // Get the OSSD1 status
+  ss.clear();
+  ss << response.substr(17, 1);
+  RCLCPP_DEBUG(logger_, "OSSD1: %s", response.substr(17, 1).c_str());
+  ss >> std::hex >> status.ossd_1;
+
+  // Get the OSSD2 status
+  ss.clear();
+  ss << response.substr(18, 1);
+  RCLCPP_DEBUG(logger_, "OSSD2: %s", response.substr(18, 1).c_str());
+  ss >> std::hex >> status.ossd_2;
+
+  // Get the Warning 1 status
+  ss.clear();
+  ss << response.substr(19, 1);
+  RCLCPP_DEBUG(logger_, "Warn1: %s", response.substr(19, 1).c_str());
+  ss >> std::hex >> status.warn_1;
+
+  // Get the Warning 2 status
+  ss.clear();
+  ss << response.substr(20, 1);
+  RCLCPP_DEBUG(logger_, "Warn2: %s", response.substr(20, 1).c_str());
+  ss >> std::hex >> status.warn_2;
+
+  // Get the OSSD3 status
+  ss.clear();
+  ss << response.substr(21, 1);
+  RCLCPP_DEBUG(logger_, "OSSD3: %s", response.substr(21, 1).c_str());
+  ss >> std::hex >> status.ossd_3;
+
+  // Get the OSSD4 status
+  ss.clear();
+  ss << response.substr(21, 1);
+  RCLCPP_DEBUG(logger_, "OSSD4: %s", response.substr(21, 1).c_str());
+  ss >> std::hex >> status.ossd_4;
+
   return true;
 }
 

src/urg_node.cpp

@@ -162,6 +162,12 @@ bool UrgNode::updateStatus()
         msg.error_status = status.error_status;
         msg.error_code = status.error_code;
         msg.lockout_status = status.lockout_status;
+        msg.ossd_1 = status.ossd_1;
+        msg.ossd_2 = status.ossd_2;
+        msg.warning_1 = status.warn_1;
+        msg.warning_2 = status.warn_2;
+        msg.ossd_3 = status.ossd_3;
+        msg.ossd_4 = status.ossd_4;
 
         lockout_status_ = status.lockout_status;
         error_code_ = status.error_code;