From 1f65d15909b7f2ec2816e0d861750c53fc67ece7 Mon Sep 17 00:00:00 2001 From: Richard Date: Fri, 24 Mar 2023 09:29:51 +0000 Subject: [PATCH 1/3] set expected AR00 DL00 sizes from UAM manual --- include/urg_node/urg_c_wrapper.hpp | 3 +++ src/urg_c_wrapper.cpp | 10 ++++++---- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/include/urg_node/urg_c_wrapper.hpp b/include/urg_node/urg_c_wrapper.hpp index 52f9f187..ba955a55 100644 --- a/include/urg_node/urg_c_wrapper.hpp +++ b/include/urg_node/urg_c_wrapper.hpp @@ -102,6 +102,9 @@ struct SerialConnection int serial_baud; }; +static const size_t AR00_PACKET_SIZE = 4379; +static const size_t DL00_PACKET_SIZE = 1936; + class URGCWrapper { public: diff --git a/src/urg_c_wrapper.cpp b/src/urg_c_wrapper.cpp index 00a7eec2..0cc86de8 100644 --- a/src/urg_c_wrapper.cpp +++ b/src/urg_c_wrapper.cpp @@ -327,8 +327,9 @@ bool URGCWrapper::getAR00Status(URGStatus & status) // Get the response std::string response = sendCommand(str_cmd); - if (response.empty()) { - RCLCPP_WARN(logger_, "Received empty response from AR00 command"); + if (response.empty() || response.size() < AR00_PACKET_SIZE) { + RCLCPP_WARN(logger_, "Invalid response from AR00 expected size: %lu actual: %lu", + AR00_PACKET_SIZE, response.size()); return false; } @@ -419,8 +420,9 @@ bool URGCWrapper::getDL00Status(UrgDetectionReport & report) // Get the response std::string response = sendCommand(str_cmd); - if (response.empty()) { - RCLCPP_WARN(logger_, "Received empty response from DL00 command"); + if (response.empty() || response.size() < DL00_PACKET_SIZE) { + RCLCPP_WARN(logger_, "Invalid response from DL00 expected size: %lu actual: %lu", + DL00_PACKET_SIZE, response.size()); return false; } From 9881d3e17c80ddf22577660e637e5fab57f28b38 Mon Sep 17 00:00:00 2001 From: Richard Date: Fri, 24 Mar 2023 10:54:54 +0000 Subject: [PATCH 2/3] fixed formatting --- src/urg_c_wrapper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/urg_c_wrapper.cpp b/src/urg_c_wrapper.cpp index 0cc86de8..539b0996 100644 --- a/src/urg_c_wrapper.cpp +++ b/src/urg_c_wrapper.cpp @@ -328,7 +328,8 @@ bool URGCWrapper::getAR00Status(URGStatus & status) std::string response = sendCommand(str_cmd); if (response.empty() || response.size() < AR00_PACKET_SIZE) { - RCLCPP_WARN(logger_, "Invalid response from AR00 expected size: %lu actual: %lu", + RCLCPP_WARN( + logger_, "Invalid response from AR00 expected size: %lu actual: %lu", AR00_PACKET_SIZE, response.size()); return false; } @@ -421,7 +422,8 @@ bool URGCWrapper::getDL00Status(UrgDetectionReport & report) std::string response = sendCommand(str_cmd); if (response.empty() || response.size() < DL00_PACKET_SIZE) { - RCLCPP_WARN(logger_, "Invalid response from DL00 expected size: %lu actual: %lu", + RCLCPP_WARN( + logger_, "Invalid response from DL00 expected size: %lu actual: %lu", DL00_PACKET_SIZE, response.size()); return false; } From 2b4e00624df6ec3a84f9b4521ab6fad2006d3cc0 Mon Sep 17 00:00:00 2001 From: Richard Date: Sat, 25 Mar 2023 12:17:23 +0000 Subject: [PATCH 3/3] added product name check for detailed status --- src/urg_node.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/urg_node.cpp b/src/urg_node.cpp index 862e3f16..8b8f3389 100644 --- a/src/urg_node.cpp +++ b/src/urg_node.cpp @@ -155,6 +155,13 @@ bool UrgNode::updateStatus() device_status_ = urg_->getSensorStatus(); if (detailed_status_) { + // check product supports this command + if (product_name_.find("UAM") != 0) { + RCLCPP_WARN( + this->get_logger(), "Detailed status only available for UAM series, your model is %s", + product_name_.c_str()); + return false; + } URGStatus status; if (urg_->getAR00Status(status)) { urg_node_msgs::msg::Status msg;