From d72bba3d40712f42252f24a64b18f0cd0843eefd Mon Sep 17 00:00:00 2001 From: bgfxc4 <50016993+bgfxc4@users.noreply.github.com> Date: Fri, 8 Nov 2024 16:36:58 +0100 Subject: [PATCH] Add dual antenna diagnostics (#124) * Update the parameters in the README * Added RXSTATUS message and new Second Antenna diagnostic --- README.md | 7 +- novatel_gps_driver/CMakeLists.txt | 1 + .../nodes/novatel_gps_node.h | 8 ++ .../include/novatel_gps_driver/novatel_gps.h | 9 ++ .../novatel_gps_driver/parsers/rxstatus.h | 58 ++++++++++ .../src/nodes/novatel_gps_node.cpp | 52 +++++++++ novatel_gps_driver/src/novatel_gps.cpp | 20 ++++ novatel_gps_driver/src/parsers/rxstatus.cpp | 102 ++++++++++++++++++ novatel_gps_msgs/CMakeLists.txt | 1 + novatel_gps_msgs/msg/NovatelRxStatus.msg | 17 +++ 10 files changed, 274 insertions(+), 1 deletion(-) create mode 100644 novatel_gps_driver/include/novatel_gps_driver/parsers/rxstatus.h create mode 100644 novatel_gps_driver/src/parsers/rxstatus.cpp create mode 100644 novatel_gps_msgs/msg/NovatelRxStatus.msg diff --git a/README.md b/README.md index f4958dc6..9bb0598a 100644 --- a/README.md +++ b/README.md @@ -134,7 +134,7 @@ Nodelets - `publish_clocksteering`: `true` to publish novatel_gps_msgs/ClockSteering messages. - Default: `false` - `publish_diagnostics`: `true` to publish node diagnostics. - - Default: `false` + - Default: `true` - `publish_gpgsa`: `true` to publish novatel_gps_msgs/Gpgsa messages. - Default: `false` - `publish_gpgsv`: `true` to publish novatel_gps_msgs/Gpgsv messages. @@ -170,10 +170,15 @@ Nodelets - `publish_sync_diagnostic`: If true, publish a time Sync diagnostic. - Ignored if `publish_diagnostics` is false. - Default: `true` + - `publish_dual_antenna_diagnostic`: If true, publish diagnostics for the second antenna. + - Ignored if `publish_diagnostics` is false. + - Default: same as `publish_novatel_dual_antenna_heading` - `publish_time_messages`: `true` to publish novatel_gps_msgs/Time messages. - Default: `false` - `publish_trackstat`: `true` to publish novatel_gps_msgs/Trackstat messages. - Default: `false` + - `publish_invalid_gpsfix`: `true` to publish the `/gps` topic, even if the status of the GPS fix is `STATUS_NO_FIX`. + - Default: `false` - `reconnect_delay_s`: Second delay between reconnection attempts. - Default: `0.5` - `serial_baud`: Select the serial baud rate to be used in a serial connection. diff --git a/novatel_gps_driver/CMakeLists.txt b/novatel_gps_driver/CMakeLists.txt index 6f0defba..adbe253b 100644 --- a/novatel_gps_driver/CMakeLists.txt +++ b/novatel_gps_driver/CMakeLists.txt @@ -60,6 +60,7 @@ add_library(${PROJECT_NAME} src/parsers/range.cpp src/parsers/time.cpp src/parsers/trackstat.cpp + src/parsers/rxstatus.cpp ) set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON) diff --git a/novatel_gps_driver/include/novatel_gps_driver/nodes/novatel_gps_node.h b/novatel_gps_driver/include/novatel_gps_driver/nodes/novatel_gps_node.h index 92705283..3f6e6fde 100644 --- a/novatel_gps_driver/include/novatel_gps_driver/nodes/novatel_gps_node.h +++ b/novatel_gps_driver/include/novatel_gps_driver/nodes/novatel_gps_node.h @@ -220,6 +220,7 @@ namespace novatel_gps_driver bool publish_trackstat_; bool publish_diagnostics_; bool publish_sync_diagnostic_; + bool publish_dual_antenna_diagnostic_; bool publish_invalid_gpsfix_; double reconnect_delay_s_; bool use_binary_messages_; @@ -288,6 +289,11 @@ namespace novatel_gps_driver int32_t measurement_count_; rclcpp::Time last_published_; novatel_gps_msgs::msg::NovatelPosition::SharedPtr last_novatel_position_; + // ROS Dual Antenna diagnostics + uint32_t aux1stat_; + uint32_t aux2stat_; + uint32_t aux3stat_; + uint32_t aux4stat_; std::string imu_frame_id_; std::string frame_id_; @@ -327,6 +333,8 @@ namespace novatel_gps_driver void RateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status); + void DualAntennaDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status); + rclcpp::Time NovatelTimeToLocalTime(const TimeParserMsgT & utc_time); }; } diff --git a/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h b/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h index 00944409..39b1a624 100644 --- a/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h +++ b/novatel_gps_driver/include/novatel_gps_driver/novatel_gps.h @@ -81,6 +81,7 @@ #include #include #include +#include namespace novatel_gps_driver { @@ -275,6 +276,12 @@ namespace novatel_gps_driver * @param[out] clocksteering_msgs New CLOCKSTEERING messages. */ void GetClockSteeringMessages(std::vector& clocksteering_msgs); + /** + * @brief Provides any RXSTATUS messages that have been received since the + * last time this was called. + * @param[out] rxstatus_msgs New RXSTATUS messages. + */ + void GetRxStatusMessages(std::vector& rxstatus_msgs); /** * @return true if we are connected to a NovAtel device, false otherwise. @@ -504,6 +511,7 @@ namespace novatel_gps_driver RangeParser range_parser_; TimeParser time_parser_; TrackstatParser trackstat_parser_; + RxStatusParser rxstatus_parser_; // Message buffers boost::circular_buffer clocksteering_msgs_; @@ -530,6 +538,7 @@ namespace novatel_gps_driver boost::circular_buffer range_msgs_; boost::circular_buffer time_msgs_; boost::circular_buffer trackstat_msgs_; + boost::circular_buffer rxstatus_msgs_; novatel_gps_driver::Psrdop2Parser::MessageType latest_psrdop2_; diff --git a/novatel_gps_driver/include/novatel_gps_driver/parsers/rxstatus.h b/novatel_gps_driver/include/novatel_gps_driver/parsers/rxstatus.h new file mode 100644 index 00000000..6d50916b --- /dev/null +++ b/novatel_gps_driver/include/novatel_gps_driver/parsers/rxstatus.h @@ -0,0 +1,58 @@ +// ***************************************************************************** +// +// Copyright (c) 2019, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#ifndef NOVATEL_GPS_DRIVER_RXSTATUS_H +#define NOVATEL_GPS_DRIVER_RXSTATUS_H + +#include + +#include +#include + +namespace novatel_gps_driver +{ + class RxStatusParser : public MessageParser + { + public: + uint32_t GetMessageId() const override; + + const std::string GetMessageName() const override; + + MessageType ParseBinary(const BinaryMessage& bin_msg) noexcept(false) override; + + MessageType ParseAscii(const NovatelSentence& sentence) noexcept(false) override; + + static constexpr uint16_t MESSAGE_ID = 93; + static constexpr size_t BINARY_LENGTH = 88; + static constexpr size_t ASCII_LENGTH = 22; + static const std::string MESSAGE_NAME; + }; +} + +#endif //NOVATEL_GPS_DRIVER_RXSTATUS_H diff --git a/novatel_gps_driver/src/nodes/novatel_gps_node.cpp b/novatel_gps_driver/src/nodes/novatel_gps_node.cpp index 8bd69069..2563eee8 100644 --- a/novatel_gps_driver/src/nodes/novatel_gps_node.cpp +++ b/novatel_gps_driver/src/nodes/novatel_gps_node.cpp @@ -27,6 +27,7 @@ // // ***************************************************************************** +#include "novatel_gps_driver/parsers/rxstatus.h" #include #include @@ -70,6 +71,7 @@ namespace novatel_gps_driver publish_trackstat_(false), publish_diagnostics_(true), publish_sync_diagnostic_(true), + publish_dual_antenna_diagnostic_(publish_novatel_dual_antenna_heading_), publish_invalid_gpsfix_(false), reconnect_delay_s_(0.5), use_binary_messages_(false), @@ -86,6 +88,10 @@ namespace novatel_gps_driver gps_insufficient_data_warnings_(0), publish_rate_warnings_(0), measurement_count_(0), + aux1stat_(-1), + aux2stat_(-1), + aux3stat_(-1), + aux4stat_(-1), last_published_(get_clock()->get_clock_type()), imu_frame_id_(""), frame_id_("") @@ -118,6 +124,7 @@ namespace novatel_gps_driver publish_trackstat_ = this->declare_parameter("publish_trackstat", publish_trackstat_); publish_diagnostics_ = this->declare_parameter("publish_diagnostics", publish_diagnostics_); publish_sync_diagnostic_ = this->declare_parameter("publish_sync_diagnostic", publish_sync_diagnostic_); + publish_dual_antenna_diagnostic_ = this->declare_parameter("publish_dual_antenna_diagnostic", publish_dual_antenna_diagnostic_); polling_period_ = this->declare_parameter("polling_period", polling_period_); expected_rate_ = this->declare_parameter("expected_rate", 1.0 / polling_period_); reconnect_delay_s_ = this->declare_parameter("reconnect_delay_s", reconnect_delay_s_); @@ -275,6 +282,13 @@ namespace novatel_gps_driver this, &NovatelGpsNode::SyncDiagnostic); } + + if (publish_dual_antenna_diagnostic_) + { + diagnostic_updater_.add("Dual Antenna", + this, + &NovatelGpsNode::DualAntennaDiagnostic); + } } @@ -386,6 +400,10 @@ namespace novatel_gps_driver { opts["trackstat" + format_suffix] = 1.0; // Trackstat } + if (publish_dual_antenna_diagnostic_ && publish_diagnostics_) + { + opts["rxstatus" + format_suffix] = 1.0; + } // Set the serial baud rate if needed if (connection_ == NovatelGps::SERIAL) { @@ -777,6 +795,18 @@ namespace novatel_gps_driver trackstat_pub_->publish(std::move(msg)); } } + if (publish_dual_antenna_diagnostic_) + { + std::vector rxstatus_msgs; + gps_.GetRxStatusMessages(rxstatus_msgs); + for (auto& msg : rxstatus_msgs) + { + aux1stat_ = msg->aux1stat; + aux2stat_ = msg->aux2stat; + aux3stat_ = msg->aux3stat; + aux4stat_ = msg->aux4stat; + } + } if (publish_imu_messages_) { std::vector novatel_imu_msgs; @@ -1168,6 +1198,28 @@ namespace novatel_gps_driver publish_rate_warnings_ = 0; } + void NovatelGpsNode::DualAntennaDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status) + { + bool powered = aux2stat_ & 0x10000000; + bool open = aux2stat_ & 0x20000000; + bool shorted = aux2stat_ & 0x40000000; + + if (open || shorted) + { + status.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Second Antenna Connection Error"); + } + else + { + status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nominal"); + } + + status.add("Second Antenna Not Powered", powered ? "false" : "true"); + status.add("Second Antenna Open", aux2stat_ & 0x20000000 ? "true" : "false"); + status.add("Second Antenna Shorted", aux2stat_ & 0x40000000 ? "true" : "false"); + + status.add("aux2stat", aux2stat_); + } + rclcpp::Time NovatelGpsNode::NovatelTimeToLocalTime(const TimeParserMsgT & time_msg) { // Build tm struct from NovatelTime diff --git a/novatel_gps_driver/src/novatel_gps.cpp b/novatel_gps_driver/src/novatel_gps.cpp index e2804e09..a7ba346c 100644 --- a/novatel_gps_driver/src/novatel_gps.cpp +++ b/novatel_gps_driver/src/novatel_gps.cpp @@ -27,6 +27,7 @@ // // ***************************************************************************** +#include "novatel_gps_driver/parsers/rxstatus.h" #include #include #include @@ -86,6 +87,7 @@ namespace novatel_gps_driver range_msgs_(MAX_BUFFER_SIZE), time_msgs_(MAX_BUFFER_SIZE), trackstat_msgs_(MAX_BUFFER_SIZE), + rxstatus_msgs_(MAX_BUFFER_SIZE), imu_rate_(-1.0), apply_vehicle_body_rotation_(false) { @@ -532,6 +534,11 @@ namespace novatel_gps_driver DrainQueue(clocksteering_msgs_, clocksteering_msgs); } + void NovatelGps::GetRxStatusMessages(std::vector& rxstatus_msgs) + { + DrainQueue(rxstatus_msgs_, rxstatus_msgs); + } + bool NovatelGps::CreatePcapConnection(const std::string& device, NovatelMessageOpts const& opts) { RCLCPP_INFO(node_.get_logger(), "Opening pcap file: %s", device.c_str()); @@ -1139,6 +1146,13 @@ namespace novatel_gps_driver trackstat_msgs_.push_back(std::move(trackstat)); break; } + case RxStatusParser::MESSAGE_ID: + { + auto rxstatus = rxstatus_parser_.ParseBinary(msg); + rxstatus->header.stamp = stamp; + rxstatus_msgs_.push_back(std::move(rxstatus)); + break; + } default: RCLCPP_WARN(node_.get_logger(), "Unexpected binary message id: %u", msg.header_.message_id_); break; @@ -1320,6 +1334,12 @@ namespace novatel_gps_driver trackstat->header.stamp = stamp; trackstat_msgs_.push_back(std::move(trackstat)); } + else if (sentence.id == "RXSTATUSA") + { + auto rxstatus = rxstatus_parser_.ParseAscii(sentence); + rxstatus->header.stamp = stamp; + rxstatus_msgs_.push_back(std::move(rxstatus)); + } else if (sentence.id == "RAWIMUXA") { static std::map> rates = { diff --git a/novatel_gps_driver/src/parsers/rxstatus.cpp b/novatel_gps_driver/src/parsers/rxstatus.cpp new file mode 100644 index 00000000..2b235b5b --- /dev/null +++ b/novatel_gps_driver/src/parsers/rxstatus.cpp @@ -0,0 +1,102 @@ +// ***************************************************************************** +// +// Copyright (c) 2019, Southwest Research Institute® (SwRI®) +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Southwest Research Institute® (SwRI®) nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +// ***************************************************************************** + +#include "novatel_gps_driver/parsers/parsing_utils.h" +#include + +#include + +#include + +namespace novatel_gps_driver +{ + const std::string RxStatusParser::MESSAGE_NAME = "RXSTATUS"; + + uint32_t RxStatusParser::GetMessageId() const + { + return MESSAGE_ID; + } + + const std::string RxStatusParser::GetMessageName() const + { + return MESSAGE_NAME; + } + + RxStatusParser::MessageType RxStatusParser::ParseBinary(const BinaryMessage& bin_msg) noexcept(false) + { + if (bin_msg.data_.size() != BINARY_LENGTH) + { + std::stringstream error; + error << "Unexpected RXSTATUS message length: " << bin_msg.data_.size(); + throw ParseException(error.str()); + } + auto ros_msg = std::make_shared(); + HeaderParser header_parser; + ros_msg->novatel_msg_header = header_parser.ParseBinary(bin_msg); + ros_msg->novatel_msg_header.message_name = MESSAGE_NAME; + + ros_msg->error = ParseUInt32(&bin_msg.data_[0]); + ros_msg->rxstat = ParseUInt32(&bin_msg.data_[8]); + ros_msg->aux1stat = ParseUInt32(&bin_msg.data_[24]); + ros_msg->aux2stat = ParseUInt32(&bin_msg.data_[40]); + ros_msg->aux3stat = ParseUInt32(&bin_msg.data_[56]); + ros_msg->aux4stat = ParseUInt32(&bin_msg.data_[72]); + return ros_msg; + } + + RxStatusParser::MessageType RxStatusParser::ParseAscii(const NovatelSentence& sentence) noexcept(false) + { + auto msg = std::make_shared(); + HeaderParser h_parser; + msg->novatel_msg_header = h_parser.ParseAscii(sentence); + + if (sentence.body.size() != ASCII_LENGTH) + { + std::stringstream error; + error << "Unexpected number of RXSTATUS message fields: " << sentence.body.size(); + throw ParseException(error.str()); + } + + bool valid = true; + + valid = valid && ParseUInt32(sentence.body[0], msg->error); + valid = valid && ParseUInt32(sentence.body[1], msg->rxstat); + valid = valid && ParseUInt32(sentence.body[6], msg->aux1stat); + valid = valid && ParseUInt32(sentence.body[10], msg->aux2stat); + valid = valid && ParseUInt32(sentence.body[14], msg->aux3stat); + valid = valid && ParseUInt32(sentence.body[18], msg->aux4stat); + + if (!valid) + { + throw ParseException("Invalid field in RXSTATUS message"); + } + + return msg; + } +} diff --git a/novatel_gps_msgs/CMakeLists.txt b/novatel_gps_msgs/CMakeLists.txt index 1652fbb0..1f433928 100644 --- a/novatel_gps_msgs/CMakeLists.txt +++ b/novatel_gps_msgs/CMakeLists.txt @@ -40,6 +40,7 @@ set(MSG_FILES msg/Time.msg msg/Trackstat.msg msg/TrackstatChannel.msg + msg/NovatelRxStatus.msg srv/NovatelFRESET.srv ) diff --git a/novatel_gps_msgs/msg/NovatelRxStatus.msg b/novatel_gps_msgs/msg/NovatelRxStatus.msg new file mode 100644 index 00000000..fb2c4c02 --- /dev/null +++ b/novatel_gps_msgs/msg/NovatelRxStatus.msg @@ -0,0 +1,17 @@ +# Based on https://docs.novatel.com/OEM7/Content/Logs/RXSTATUS.htm +std_msgs/Header header + +NovatelMessageHeader novatel_msg_header + +# for the bit table of the following values see https://docs.novatel.com/OEM7/Content/Logs/RXSTATUS.htm +uint32 error + +uint32 rxstat + +uint32 aux1stat + +uint32 aux2stat + +uint32 aux3stat + +uint32 aux4stat