diff --git a/include/urg_node/urg_c_wrapper.hpp b/include/urg_node/urg_c_wrapper.hpp index 52f9f187..f469ef95 100644 --- a/include/urg_node/urg_c_wrapper.hpp +++ b/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 diff --git a/launch/urg_lidar.launch b/launch/urg_lidar.launch deleted file mode 100644 index e729ac2f..00000000 --- a/launch/urg_lidar.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/launch/urg_lidar.launch.xml b/launch/urg_lidar.launch.xml new file mode 100644 index 00000000..e023667c --- /dev/null +++ b/launch/urg_lidar.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + diff --git a/launch/urg_node_launch.py b/launch/urg_node_launch.py index 21222f9c..862e7a4d 100644 --- a/launch/urg_node_launch.py +++ b/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')] ) diff --git a/src/urg_c_wrapper.cpp b/src/urg_c_wrapper.cpp index c6b5033d..d037a9db 100644 --- a/src/urg_c_wrapper.cpp +++ b/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; } diff --git a/src/urg_node.cpp b/src/urg_node.cpp index c0d16420..2d5a16e1 100644 --- a/src/urg_node.cpp +++ b/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;